【ROS1仿真】小车跟随（总体流程）_跟随小车

第一篇博客给出了总体代码：https://blog.csdn.net/m0_71523511/article/details/135610191
第二篇博客改善了跟随的效果：https://blog.csdn.net/m0_71523511/article/details/135675446

1、gazebo仿真世界

首先需要准备好一个仿真世界，可以自己使用gazebo搭建，也可以下载别人搭建好的，这里直接下载别人的。

    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find urdf02_gazebo)/worlds/box_house.world" />
    </include>
1
2
3

2、机器人模型

URDF 用于创建机器人模型、Rviz 可以显示机器人感知到的环境信息，Gazebo 用于仿真。通过 Gazebo 模拟机器人的传感器，然后在 Rviz 中显示这些传感器感知到的数据。下面以小车1为例子：
①单独编写各部分的xacro文件：
demo05_car_base.urdf.xacro
demo06_car_camera.urdf.xacro
demo07_car_laser.urdf.xacro
将上述的文件集合起来就可以组成机器人模型，但是这样在gazebo中是无法进行仿真的，还需要为 joint 添加传动装置以及控制器、配置雷达传感器信息、配置摄像头传感器信息、配置 kinetic传感器信息，实际上小车跟随没有用到摄像头和kinetic，但是为了扩展方便也就一起放进去了，这样如果想看看摄像头拍到了什么，就只需要在rviz中添加摄像头然后订阅对应话题（/camera/image_raw）就可以看到摄像头输出的图像了。
②为 joint 添加传动装置以及控制器、配置雷达传感器信息、配置摄像头传感器信息、配置 kinetic传感器信息：
move.xacro
laser.xacro
camera.xacro
kinetic.xacro
③将所有xacro文件集成进总的机器人模型文件
car.urdf.xacro
这里所有的文件全部位于：urdf02_gazebo/urdf下。小车2的模型文件位于urdf02_gazebo/urdf2下。

3、slam建图

SLAM算法有多种，这里选用gmapping，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图。
①可以直接用总的launch文件建图，注释里面的读取地图代码：

<include file="$(find nav_demo)/launch/nav03_readmap.launch" />
1

因为在写总的launch文件之前我就已经把地图建好了，所以直接用上面这个注释的launch文件读取地图。注意将这段代码注释完之后，进入rviz需要将地图订阅的话题改成/robot1/map，这样就可以控制小车1运动，小车2自动跟随然后建图。
建完之后执行保存地图的launch文件：

<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)">
        <remap from="map" to="robot1/map"/>  <!-- 添加这一行来重映射话题 -->
    </node>
</launch>
1
2
3
4
5
6

位于nav_demo/launch/nav02_savemap.launch,保存完之后就可以将总体launch文件中注释的代码释放，然后进入rviz中将地图中订阅的话题改成/map。
②不使用总体launch文件，而是使用单独的小车进行建图：
直接看文档的7.2.1，给出了launch文件，复制执行即可。注意可能需要改些东西。

4、定位

所谓定位就是推算机器人自身在全局地图中的位置，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中，导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。
这里只有小车1需要用到amcl，单独编写一个launch文件然后在总的launch文件中调用即可。可以在rviz中添加posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；

5、路径规划

路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。代价地图的概念和碰撞算法的概念都可以直接看文档7.2.4。
要使用move_base 需要许多配置文件：
costmap_common_params_burger.yaml
local_costmap_params.yaml
global_costmap_params.yaml
base_local_planner_params.yaml
各个配置文件的作用在后面的代码标注中有说明。在rviz中可以再添加两个地图，分别订阅全局和局部地图，就可以看到代价地图。

6、小车跟随

由上述路径规划可知move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，这样我只需要将小车2相对于robot2/base_footprint的相对位置关系当作此目标点，即可实现小车1跟随小车2的效果，因为小车1有一个话题/robot1/move_base_simple/goal会接收/test03_control_turtle2节点的信息，以此控制小车1向这个目标点运动（/test03_control_turtle2这个节点就是自己实现的用于控制跟随的核心）。
/test03_control_turtle2这个节点的代码在之前发的第一篇博客中给出了。

7、总体launch文件

使用rostopic echo /tf就可以查看此时各坐标系之间的坐标变换。这是由ros中的tf自动计算并发布的。
①小车1：

   <group ns="robot1">
        <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />  //导入小车1模型
        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />//发布机器人的关节状态，使其他ROS节点或工具可以订阅这些状态信息，从而了解机器人当前的姿态和关节状态。
        <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" >//这段代码的目的也是发布机器人的关节状态。关节状态是机器人控制和运动的关键信息，包括每个关节的位置、速度和可能的其他属性。话题名称：/robot1/joint_state
            <param name="publish_frequency" value="10.0" /> //设定频率慢一些，否则会有很多警告
        </node>
        <include file="$(find nav_demo)/launch/nav04_amcl_t.launch" />  //amcl功能包相关参数设置
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">  //路径规划功能包相关设置
            <rosparam file="$(find nav_demo)/param/costmap_common_params_t.yaml" command="load" ns="global_costmap" />  //在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。
            <rosparam file="$(find nav_demo)/param/costmap_common_params_t.yaml" command="load" ns="local_costmap" /> 
            <rosparam file="$(find nav_demo)/param/local_costmap_params_t.yaml" command="load" /> //局部代价地图参数设置
            <rosparam file="$(find nav_demo)/param/global_costmap_params_t.yaml" command="load" /> //全局代价地图参数设置
            <rosparam file="$(find nav_demo)/param/base_local_planner_params_t.yaml" command="load" /> //基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。
        </node>
        <remap from="/move_base_simple/goal" to="/robot1/move_base_simple/goal"/> //将导航位置的落点话题remap成/robot1/move_base_simple/goal，这样小车1才能正确接收落点信息。
        <param name="use_sim_time" value="true"/>  //表示使用模拟
        <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping1" output="screen">  //slam建图相关参数设置。
            <remap from="scan" to="scan"/>
            <param name="base_frame" value="robot1/base_footprint"/>
            <param name="map_frame" value="map"/>
            <param name="odom_frame" value="robot1/odom"/>
            <param name="map_topic" value="map"/>
            <param name="map_update_interval" value="5.0"/>
            <param name="maxUrange" value="16.0"/>
            <param name="sigma" value="0.05"/>
            <param name="kernelSize" value="1"/>
            <param name="lstep" value="0.05"/>
            <param name="astep" value="0.05"/>
            <param name="iterations" value="5"/>
            <param name="lsigma" value="0.075"/>
            <param name="ogain" value="3.0"/>
            <param name="lskip" value="0"/>
            <param name="srr" value="0.1"/>
            <param name="srt" value="0.2"/>
            <param name="str" value="0.1"/>
            <param name="stt" value="0.2"/>
            <param name="linearUpdate" value="1.0"/>
            <param name="angularUpdate" value="0.5"/>
            <param name="temporalUpdate" value="3.0"/>
            <param name="resampleThreshold" value="0.5"/>
            <param name="particles" value="30"/>
            <param name="xmin" value="-50.0"/>
            <param name="ymin" value="-50.0"/>
            <param name="xmax" value="50.0"/>
            <param name="ymax" value="50.0"/>
            <param name="delta" value="0.05"/>
            <param name="llsamplerange" value="0.01"/>
            <param name="llsamplestep" value="0.01"/>
            <param name="lasamplerange" value="0.005"/>
            <param name="lasamplestep" value="0.005"/>
        </node>
        <!-- 其他robot1的节点，如gmapping等 -->
    </group>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

②小车2：

    <group ns="robot2">
        <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf2/car2.urdf.xacro" />
        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
        <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" >
            <param name="publish_frequency" value="10.0" /> 
        </node>
        <param name="use_sim_time" value="true"/>
        <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping2" output="screen">  //这里不用gmapping功能包的话就没法创建小车模型，不知道原因，应该和坐标系有关系。
            <param name="base_frame" value="robot2/base_footprint"/>//通过这些参数，slam_gmapping节点知道如何处理和转换不同坐标系之间的关系。它会自动发布基于激光雷达扫描和里程计信息的坐标变换。ROS中的TF（Transform）系统会自动处理这些坐标系之间的变换，并发布相应的TF信息。所以，与这段代码相关的坐标变换将由ROS中的TF系统自动处理。
            <param name="map_frame" value="map"/>
            <param name="odom_frame" value="robot2/odom"/>
        </node>
    </group>
1
2
3
4
5
6
7
8
9
10
11
12
13

③gazebo中启动两个小车模型：

    <node pkg="gazebo_ros" type="spawn_model" name="spawn_model_2" args="-urdf -model car2 -param robot2/robot_description" />
    <node pkg="gazebo_ros" type="spawn_model" name="spawn_model_1" args="-urdf -model car1 -param robot1/robot_description" />```

1
2
3