重操旧业，最近实验室又需要测试无人车导航算法，因此又重新启动了松灵机器人scout mini小车 自主导航项目。通过调研，最终选择了前几年比较火的CMU团队的策略(https://www.cmu-exploration.com/)。
CMU exploration 介绍
CMU exploration是一项非常优秀的开源工作，得到了广泛的认可。作者等人维护了一个社区用于解答提问，详细可见链接
参考引用
算法方面我就不做过多的介绍了，详细可见对应的论文和代码地址，本文只介绍在仿真配置流程和过程中遇到的一系列问题。
- 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
AI写代码
bash
-
# 案例：发送一系列 waypoint，同时发送导航边界和速度，车辆在跟踪 waypoint 的同时在边界内行驶（另开一个终端）
-
cd autonomous_exploration_development_environment
-
source devel/setup.sh
-
roslaunch waypoint_example waypoint_example_garage.launch
-
AI写代码
bash
规划算法搭建
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
AI写代码
bash
- 方法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
-
AI写代码
bash
仿真启动效果
启动 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>
-
AI写代码
bash
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
AI写代码
bash
-
启动scout 仿真
-
启动LIO-SAM
-
启动 system_real_robot.launch
-
启动 far planner
- 导航效果图
遇到问题汇总
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.AI写代码 bash
在实际操作中发现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;
-
}
AI写代码
c++
运行
重新编译即可。
原创作者: binbin2002 转载于: https://www.cnblogs.com/binbin2002/p/18847949
已为社区贡献6条内容
所有评论(0)