ROS2版Turbot3-Multi仿真教程-3台机器人frontier自主探索建图

说明

• 介绍实现2台机器人探索建图，也支持导航，键盘控制，并合并大地图
• 环境：ubuntu22.04 + ros2 humble

初始化机器人在仿真地图上的位置

• 初始化机器人在仿真地图上的位置， 打开robots_world_house_merge_012.json

vim ~/ros2_tb3_multi_ws/src/turtlebot3_multi/config/robots_world_house_merge_012.json

• 文件内容

[
  {
    "name": "tb3_0",
    "x_pose": 3.0,
    "y_pose": 2.0,
    "z_pose": 0.0,
    "a": 0.0,
    "roll": 0.0,
    "pitch": 0.0,
    "yaw": 0.0
  },
  {
    "name": "tb3_1",
    "x_pose": -1.0,
    "y_pose": 2.0,
    "z_pose": 0.0,
    "a": 0.0,
    "roll": 0.0,
    "pitch": 0.0,
    "yaw": 0.0
  },
  {
    "name": "tb3_2",
    "x_pose": 1.0,
    "y_pose": 3.0,
    "z_pose": 0.0,
    "a": 0.0,
    "roll": 0.0,
    "pitch": 0.0,
    "yaw": 0.0
  }
]

初始化机器人在合并地图上的位置

• 初始化机器人在合并地图上的位置， 打开map_merge_params.yaml

vim ~/ros2_tb3_multi_ws/src/turtlebot3_multi/config/map_merge_params_012.yaml

• 根据仿真坐标点设置合并后地图的初始化位置
• 公式如下

取两台机器人直线的中间位置为中心坐标点，X除于10,Y除于10, A坐标的都是正值，B坐标点都是负值。
第三台或更多台机器人则使用中心点到机器人坐标点的相对坐标点
比如 A:x=3.0,y=2.0  B:x=-1.0,y=2.0 C:x=1, y=3.0 得到的合并地图坐标。A:x=0.2,y=0.0  B:x=-0.2,y=0.0 C: x=0.0,y=-0.1

Xb1=(abs(X1-X2)/2)/10   
Yb1=(abs(Y1-Y2)/2)/10

Xb2=-(abs(X1-X2)/2)/10
Yb2=-(abs(Y1-Y2)/2)/10

Xb3=(X3-(abs(X1-abs((X1-X2)/2))) )/10
Xb3=((abs(Y1+abs((Y1-Y2)/2))) - Y3)/10

• 默认坐标

map_merge:
  ros__parameters:
    merging_rate: 4.0
    discovery_rate: 0.5
    estimation_rate: 0.5
    estimation_confidence: 0.6
    robot_map_topic: map
    robot_map_updates_topic: map_udpates
    robot_namespace: ""
    merged_map_topic: map
    world_frame: map
    known_init_poses: true
    # known_init_poses: false

    # Define here robots' positions in the map if known_init_poses is true
    /tb3_0/map_merge/init_pose_x: 0.2
    /tb3_0/map_merge/init_pose_y: 0.0
    /tb3_0/map_merge/init_pose_z: 0.0
    /tb3_0/map_merge/init_pose_yaw: 0.0

    /tb3_1/map_merge/init_pose_x: -0.2
    /tb3_1/map_merge/init_pose_y: -0.0
    /tb3_1/map_merge/init_pose_z: 0.0
    /tb3_1/map_merge/init_pose_yaw: 0.0

    /tb3_2/map_merge/init_pose_x: 0.0
    /tb3_2/map_merge/init_pose_y: -0.1
    /tb3_2/map_merge/init_pose_z: 0.0
    /tb3_2/map_merge/init_pose_yaw: 0.0   

• 通过这个方法合并后地图的width和height接近384*384，这样合并的地图拼合才会比较完好。

启动步骤

• 新开终端，启动gazebo

./multi_scripts/multi_gazebo_3.sh robots_world_house_merge_012.json turtlebot3_house.world

• 指定坐标点，指定仿真地图
• 效果图：

• 新开终端，启动3台小车的导航

./multi_scripts/multi_nav.sh 3

• 新开终端，启动3台小车的建图

./multi_scripts/multi_slam.sh 3

• 这一步要确认运行multi_nav.sh的终端下，三台机器人导航都启动成功
• 如下出现这三个行记录即代表成功，否则要重新执行

[lifecycle_manager-27] [INFO] [1682585438.071666918] [tb3_2_lifecycle_manager_pathplanner]: Managed nodes are active
[lifecycle_manager-11] [INFO] [1682585438.677464785] [tb3_0_lifecycle_manager_pathplanner]: Managed nodes are active
[lifecycle_manager-19] [INFO] [1682585438.731784450] [tb3_1_lifecycle_manager_pathplanner]: Managed nodes are active

• 新开终端，启动rviz

./multi_scripts/multi_rviz.sh 3

• 新开终端，启动合并地图， 并指定合并后机器人的初始化坐标点

./multi_scripts/multi_map_merge.sh map_merge_params_012.yaml 

• rviz显示机器人1地图

• rviz显示机器人2地图

• rviz显示机器人3地图

• rviz显示合并地图

通过探索建图

• 启动探索程序

./multi_scripts/multi_explore.sh 3

• 导航中的效果图：

• 完成后的合并效果图

停止和开启探索

• 如果想停止探索，执行命令

ros2 topic pub -1 /tb3_0/explore/resume std_msgs/msg/Bool "{data: False}" #停止探索

• 如果机器人停止探索，再次激活

ros2 topic pub -1 /tb3_0/explore/resume std_msgs/msg/Bool "{data: True}" #开始探索

• 指定具体的机器人tb3_0、tb3_1、tb3_2

保存地图

• 建图完成之后，保存地图

./multi_scripts/multi_save_map.sh explore3

• 地图自动保存在~/ros2_tb3_multi_ws/src/turtlebot3_multi/map目录下，名为explore3.yaml和explore3.pgm

• 查看地图

eog ~/ros2_tb3_multi_ws/src/turtlebot3_multi/map/explore3.pgm

• 效果图

演示视频