多车pure_pursuit功能演示(C++)演示

本教程将会引导读者完成多车pure_pursuit功能演示(C++) 的演示。在使用此功能之前，确保已经完成了 Demo前期准备工作

提示

启动此Demo前，请关闭所有终端

打开终端，输入命令，启动 roscore ：

roscore

启动相机驱动。打开终端，输入命令：

rosrun camtoros camtoros

启动二维码检测功能包。打开终端，输入命令：

roslaunch apriltag_ros continuous_detection.launch

将1，2，3号小车开机，放置位置如图1所示：

图1 小车位置摆放

注意

小车 开关方向 为小车正方向

图示位置 左下角 为原点，右边为 x 轴正方向，上边为 y 轴正方向

摆放的时候小车左右间距尽量大一点， 太近可能会导致刚启动的时候发生碰撞

提示

小车的摆放位置取决于航点的定义，注意不要让航点产生交叉。此Demo集群小车不具备避障功能

启动通信节点。打开终端，输入命令：

rosrun tcp_communication tcpDriverNode

启动定位系统节点。打开 kk-robot-swarm/src/global_vision/launch/start.launch文件，将 car_total_num 与 car_num 改为 3，如下所示：

<launch>
  <node pkg="global_vision" type="odometryPub" respawn="false" name="odometryPub" output="screen">
     <param name="car_total_num" value="3" />
  </node>

  <node pkg="global_vision" type="static_tf" respawn="false" name="static_tf" output="screen">
     <param name="car_num" value="3" />
  </node>
  <!-- tf publisher -->
  <!-- matrix (1, 0, 0, 0, -1, 0, 0, 0, -1) -->
  <node pkg="tf" type="static_transform_publisher" name="map_hik_broadcaster" args="0 2 4 1 0 0 0 map hik_camera 100" />

</launch>

最后启动定位系统。打开终端，输入命令：

roslaunch global_vision start.launch

打开 rviz。打开终端，输入命令：

roscd camtoros/config
rviz -d tf.rviz

在rviz中加载航点。首先打开 kk-robot-swarm/src/swarm/waypoint_loader/launch/waypoint_loader.launch文件。如下所示：

<?xml version="1.0"?>
 <launch>
     <node pkg="waypoint_loader" type="waypoint_loader.py" name="waypoint_loader_1" output="screen">
         <param name="path" value="$(find waypoint_loader)/waypoints/robot_1.csv" />
         <param name="velocity" value="0.2" />
         <remap from="/path" to="/robot_1/path" />
     </node>

     <node pkg="waypoint_loader" type="waypoint_loader.py" name="waypoint_loader_2" output="screen">
         <param name="path" value="$(find waypoint_loader)/waypoints/robot_2.csv" />
         <param name="velocity" value="0.2" />
         <remap from="/path" to="/robot_2/path" />
     </node>

     <node pkg="waypoint_loader" type="waypoint_loader.py" name="waypoint_loader_3" output="screen">
         <param name="path" value="$(find waypoint_loader)/waypoints/robot_3.csv" />
         <param name="velocity" value="0.2" />
         <remap from="/path" to="/robot_3/path" />
     </node>
 </launch>

然后打开 kk-robot-swarm/src/swarm/waypoint_loader/waypoints/ 文件夹。在该文件夹下可以看到预定义的航点信息。

最后打开终端，输入命令：

roslaunch waypoint_loader waypoint_loader.launch

修改航路点文件(以 robot_1.csv 为例)

打开 kk-robot-swarm/src/swarm/waypoint_loader/waypoints/robot_1.csv 文件内容如下：
```
0.2,0.2,0.1
0.4,0.6,0.1
0.2,1.2,0.1
```
每一行代表一个航点的(x,y)坐标，第三列代表航向。(暂时用不到)

启动pure_pursuit功能包。打开 kk-robot-swarm/src/swarm/pursuit/launch/pure_pursuit.launch文件，如下所示：

<launch> 

    <arg name="name_1" default="robot_1"/>
    <arg name="name_2" default="robot_2"/>
    <arg name="name_3" default="robot_3"/>
    
    <!-- Pure pursuit path tracking -->
    <node pkg="pure_pursuit" type="pure_pursuit" name="controller_1" output="screen">
    <rosparam file="$(find pure_pursuit)/config/$(arg name_1).yaml" command="load"/>
        <remap from="path_segment" to="$(arg name_1)/path"/>
        <remap from="odometry" to="$(arg name_1)/pose"/>
        <remap from="cmd_vel" to="$(arg name_1)/cmd_vel"/>
    </node>
    <!-- add more params and nodes-->

    <node pkg="pure_pursuit" type="pure_pursuit" name="controller_2" output="screen">
    <rosparam file="$(find pure_pursuit)/config/$(arg name_2).yaml" command="load"/>
        <remap from="path_segment" to="$(arg name_2)/path"/>
        <remap from="odometry" to="$(arg name_2)/pose"/>
        <remap from="cmd_vel" to="$(arg name_2)/cmd_vel"/>
    </node>

    <node pkg="pure_pursuit" type="pure_pursuit" name="controller_3" output="screen">
        <rosparam file="$(find pure_pursuit)/config/$(arg name_3).yaml" command="load"/>
        <remap from="path_segment" to="$(arg name_3)/path"/>
        <remap from="odometry" to="$(arg name_3)/pose"/>
        <remap from="cmd_vel" to="$(arg name_3)/cmd_vel"/>
    </node>

</launch>

打开终端，输入命令：

roslaunch pure_pursuit pure_pursuit.launch

警告

此Demo不带有避障功能，如果路径发生交叉，小车将会发生碰撞

停车。首先 关闭第10步 的程序，然后打开终端，输入：

rosrun global_vision stop

视频演示

提示

如果无法播放视频，按下 ctrl + F5 刷新一下缓存即可

KKSwarm Wiki

多车pure_pursuit功能演示(C++)演示

视频演示