松灵机器人scout mini 自主导航(5)——采用CMU团队导航策略

重操旧业，最近实验室又需要测试无人车导航算法，因此又重新启动了松灵机器人scout mini小车 自主导航项目。通过调研，最终选择了前几年比较火的CMU团队的策略(https://www.cmu-exploration.com/)。

CMU exploration 介绍

CMU exploration是一项非常优秀的开源工作，得到了广泛的认可。作者等人维护了一个社区用于解答提问，详细可见链接

参考引用

• https://bbs.csdn.net/forums/TARE
• https://blog.csdn.net/qq_42994487/article/details/130307410

算法方面我就不做过多的介绍了，详细可见对应的论文和代码地址，本文只介绍在仿真配置流程和过程中遇到的一系列问题。

• ICRA 2022：Autonomous Exploration Development Environment and the Planning Algorithms
• IROS 2022：FAR planner: Fast, attemptable route planner using dynamic visibility update
  
  FAR planner

仿真小车搭建

在之前的文章中我们已经实现了小车仿真环境搭建，并成功实现了LIO-SAM算法仿真。详细内容可以参考链接

https://www.cnblogs.com/binbin2002/p/18499013

导航环境搭建

CMU团队开发了一个详细的仿真环境，；里面包含室内环境、校园、停车场、隧道、森林等仿真环境。

#  1.安装依赖
sudo apt update
sudo apt install libusb-dev

# 2. 克隆源码
git clone https://github.com/HongbiaoZ/autonomous_exploration_development_environment.git

# 3.分支检查与编译安装
cd autonomous_exploration_development_environment
git checkout melodic # Ubuntu 20.04 则将 melodic 替换为 noetic
catkin_make

# 4. 通过脚本安装仿真环境
./src/vehicle_simulator/mesh/download_environments.sh

# 5. 启动开发环境
source devel/setup.sh
roslaunch vehicle_simulator system_xxx.launch
# xxx 可替换为对应的 5 种环境之一：campus、forest、garage、indoor、tunnel

# 案例：发送一系列 waypoint，同时发送导航边界和速度，车辆在跟踪 waypoint 的同时在边界内行驶（另开一个终端）
cd autonomous_exploration_development_environment
source devel/setup.sh
roslaunch waypoint_example waypoint_example_garage.launch

规划算法搭建

CMU提供了多个导航算法，例如far_planner tare_planner，DSVP等导航策略。可以根据需求自己选择规划方法

• 方法1 ：采用tare_planner

# 1. 克隆源码
git clone https://github.com/caochao39/tare_planner.git

# 2. 编译安装
cd tare_planner
catkin_make

# 3. 启动开发环境
cd ~/autonomous_exploration_development_environment
source devel/setup.sh
roslaunch vehicle_simulator system_xxx.launch
# xxx 可替换为对应的 5 种环境之一：campus、forest、garage、indoor、tunnel

# 4. 启动 TARE（另开一个终端）
cd ~/tare_planner
source devel/setup.sh
roslaunch tare_planner explore_xxx.launch

• 方法2：采用 far_planner

# 1. 克隆源码
git clone https://github.com/MichaelFYang/far_planner

# 2. 编译安装
cd far_planner
catkin_make

# 3. 启动开发环境
cd ~/autonomous_exploration_development_environment
source devel/setup.sh
roslaunch vehicle_simulator system_xxx.launch
# xxx 可替换为对应的 5 种环境之一：campus、forest、garage、indoor、tunnel

# 4. 启动 FAR Planner（另开一个终端）
cd ~/far_planner
source devel/setup.sh
roslaunch far_planner far_planner.launch

仿真启动效果

启动 autonomous_exploration_development_environment(以garage为例)

在system_garage.launch中，修改 gazebo_gui为true，可以启动gazebo环境仿真

启动仿真环境效果如下：

启动 far_planner 规划

启动 far_planner 效果如下：

可以通过rviz选择goalpoint 用于导航

右边的按钮作用如下:

• Reset Visibility Graph：
  • 重置可视图. 算法中会提供各个环境中已经探索完的全局V-graph，可以进行重置可视图。同时可以通过勾选 Update Visibility Graph，动态更新可试图。
  • 如果想read V-graph 进来然后快速的进行规划和导航操作。此时便可把 Update Visibility Graph 取消勾选（如果环境是动态的则需要勾选以实现更新）
• Resume Navigation to Goal:
  • 重新设置导航目标点
• Read & Save:
  • 保存已经探索生成的V-Graph / 读取之前保存的V-Graph
• Planning Attemptable:
  • 如果取消勾选，则系统默认只会规划已知环境的路径（不会去尝试未知区域），当把 Goalpoint 放在未知区域时才会自动切换为 Attemptable 模式下的规划

CMU exploration与scout mini 小车+LIO-SAM结合

修改loam_interface.launch 接口

算法还提供了system_real_robot.launch能够直接进行实车部署。实车和用他自带的仿真环境在于定位方法不一定。

• gazebo仿真中采用的时gazebo自带的真实位置定位
• 实车则采用SLAM算法进行定位和建图

幸运的是，他们开放了SLAM外部接口。能够兼容目前市场上主流的激光SLAM算法：LOAM、A-LOAM、LeGO-LOAM、LIO-SAM、LIO-mapping、FAST-LIO2、Faster-LIO

• stateEstimationTopic = /lio_sam/mapping/odometry
• registeredScanTopic = /lio_sam/mapping/cloud_registered
• flipStateEstimation = false
• flipRegisteredScan = false

下面我们以之前的LIO-SAM为例进行说明

<!-- loam-interface.launch内容如下 -->
<launch>

  <node pkg="loam_interface" type="loamInterface" name="loamInterface" output="screen" required="true">
    <param name="stateEstimationTopic" type="string" value="/lio_sam/mapping/odometry" />
    <param name="registeredScanTopic" type="string" value="/lio_sam/mapping/cloud_registered" />

     <!-- 不同的slam算法发布的odometry以及registered cloud坐标系不一样，需要修改话题名称。而且，不同坐标系的方向不一样，有时候需要进行坐标旋转转换，需要坐标转换则需要修改flipStateEstimation 为true -->

    <param name="flipStateEstimation" type="bool" value="false" />
    <param name="flipRegisteredScan" type="bool" value="false" />
    <param name="sendTF" type="bool" value="true" />
    <param name="reverseTF" type="bool" value="false" />
  </node>

</launch>

loam_interface相关设置链接: https://pan.baidu.com/s/14zw6TNMD4kDjT5vkh2tgmw?pwd=d72x

提取码:d72x

仿真运行

# 1.启动scout mini 仿真
cd scout_gazebo_sim
source devel/setup.bash
roslaunch scout_gazebo_sim scout_mini_playpen.launch

# 2.启动LIO-SAM
source devel/setup.bash
roslaunch lio_sam run.launch

# 3.启动 CMU_explaration real_robot
cd CMU_exploaration
source devel/setup.bash
roslaunch vehicle_simulator system_real_robot.launch 

# 4.启动 far_planner
source devel/setup.bash
roslaunch far_planner  far_planner.launch

1. 启动scout 仿真
   
   

2. 启动LIO-SAM
   
   

3. 启动 system_real_robot.launch
   
   

4. 启动 far planner

1. 导航效果图
   
   

遇到问题汇总

1. cmd_vel 话题类型不对

[ERROR] [1745573906.144386448]: Client [/scout_skid_steer_controller] wants topic /cmd_vel to have datatype/md5sum [geometry_msgs/Twist/9f195f881246fdfa2798d1d3eebca84a], but our version has [geometry_msgs/TwistStamped/98d34b0043a2093cf9d9345ab6eef12e]. Dropping connection.

在实际操作中发现cmd_vel话题不对，询问chatgpt发现是cmd_vel的类型不对，一个是geometry_msgs/Twist类型，一个是geometry_msgs/TwistStamped 类型。我们需要保证消息类型一致

解决方法： 修改src/autonomous_exploration_development_environment/src/local_planner/src/pathFollower.cpp中的cmd_vel的类型。

//pathFollower.cpp 的226-229行
    // ros::Publisher pubSpeed = nh.advertise<geometry_msgs::TwistStamped> ("/cmd_vel", 5);
    // geometry_msgs::TwistStamped cmd_vel;
    // cmd_vel.header.frame_id = "vehicle";
    ros::Publisher pubSpeed = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
    geometry_msgs::Twist cmd_vel;

//pathFollower.cpp 的335-343行
    // if (pubSkipCount < 0) {
      //   cmd_vel.header.stamp = ros::Time().fromSec(odomTime);
      //   if (fabs(vehicleSpeed) <= maxAccel / 100.0) cmd_vel.twist.linear.x = 0;
      //   else cmd_vel.twist.linear.x = vehicleSpeed;
      //   cmd_vel.twist.angular.z = vehicleYawRate;
      //   pubSpeed.publish(cmd_vel);

      //   pubSkipCount = pubSkipNum;
      // }
      if (pubSkipCount < 0) {
        if (fabs(vehicleSpeed) <= maxAccel / 100.0) cmd_vel.linear.x = 0;
        else cmd_vel.linear.x = vehicleSpeed;
        cmd_vel.angular.z = vehicleYawRate;
        pubSpeed.publish(cmd_vel);

        pubSkipCount = pubSkipNum;
      }

重新编译即可。