ROS学习日志：话题的方式统一控制5台机器人

在gazebo环境中搭建一个尽量美观的场景、启动5台 turtlebot3 机器人 ，以话题的方式统一控制5台机器人的运动速度

（1）原有文件基础上添加多个机器人

（2）统一话题控制五个机器人(三种方式)

（3）演示视频

（4）gazebo环境中搭建场景

一、原有文件基础上添加多个机器人

1）查找该文件的路径

find -name multi_turtlebot3.launch 

2）打开编辑该文件(以已有路径为例)

gedit ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3.launch

3）增加两机器人(以加一个为例)

<arg name="forth_tb3"  default="tb3_3"/>

  <arg name="forth_tb3_x_pos" default=" 2.0"/>
  <arg name="forth_tb3_y_pos" default=" -1.0"/>
  <arg name="forth_tb3_z_pos" default=" 0.0"/>
  <arg name="forth_tb3_yaw"   default=" 0.0"/>

  <group ns = "$(arg forth_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg forth_tb3)" />
    </node>
    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg forth_tb3) -x $(arg forth_tb3_x_pos) -y $(arg forth_tb3_y_pos) -z $(arg forth_tb3_z_pos) -Y $(arg forth_tb3_yaw) -param robot_description" />
  </group>

二、统一话题控制五个机器人(三种方式)

1、第一种：修改控制驱动插件代码

1）打开编辑该文件

gedit ~/catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro

2）在机器人配置文件控制驱动插件代码中设置全局名称**/cmd_vel**

  <gazebo>
    <plugin name="mecanum_controller" filename="libgazebo_ros_planar_move.so">
      <commandTopic>/cmd_vel</commandTopic>
      <odometryTopic>/odom</odometryTopic>
      <odometryFrame>odom</odometryFrame> 	<leftFrontJoint>wheel_lf_joint</leftFrontJoint>
      <rightFrontJoint>wheel_rf_joint</rightFrontJoint>
      <leftRearJoint>wheel_lb_joint</leftRearJoint>
      <rightRearJoint>wheel_rb_joint</rightRearJoint>
      <odometryRate>20.0</odometryRate>
      <robotBaseFrame>base_link</robotBaseFrame>
    </plugin>
  </gazebo> 

3）在键盘控制节点中发布全局名称/cmd_vel，打开编译该文件

gedit ~/catkin_ws/src/turtlebot3/turtlebot3_teleop/nodes/turtlebot3_teleop_key

打开文件后更改为

pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

4）仿真运行

##新终端

roslaunch turtlebot3_gazebo multi_turtlebot3.launch

##新终端

rosrun turtlebot3_teleop turtlebot3_teleop_key

2、第二种：键盘发布器同时发布五个话题

1）在键盘控制节点中发布全局名称/cmd_vel，打开编译该文件

gedit ~/catkin_ws/src/turtlebot3/turtlebot3_teleop/nodes/turtlebot3_teleop_key

2）修改如下

pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
pub0 = rospy.Publisher('/tb3_0/cmd_vel', Twist, queue_size=10)
pub1 = rospy.Publisher('/tb3_1/cmd_vel', Twist, queue_size=10)
pub2 = rospy.Publisher('/tb3_2/cmd_vel', Twist, queue_size=10)
pub3 = rospy.Publisher('/tb3_3/cmd_vel', Twist, queue_size=10)
pub4 = rospy.Publisher('/tb3_4/cmd_vel', Twist, queue_size=10)

pub.publish(twist)
pub0.publish(twist)
pub1.publish(twist)
pub2.publish(twist)
pub3.publish(twist)
pub4.publish(twist)

3）仿真运行

##新终端

roslaunch turtlebot3_gazebo multi_turtlebot3.launch

##新终端

rosrun turtlebot3_teleop turtlebot3_teleop_key

3、第三种：新建测试节点（最实用）

原理如下：

1）创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_four roscpp rospy std_msgs turtlesim

2）配置CMakeLists.txt中的编译规则

在CMakeLists.txt中第153行后添加编译规则

add_executable(four_subscriber src/four_subscriber.cpp)
target_link_libraries(four_subscriber ${catkin_LIBRARIES})

3）在src文件夹中创建cpp文件

在learing_four功能包下的src文件夹中打开终端输入以下命令

touch four_subscriber.cpp

4）编写代码

/**
* 该例程将订阅/cmd_vel话题，消息类型geometry_msgs::Twist
*/

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
  
ros::Publisher pub0;
ros::Publisher pub1;
ros::Publisher pub2;
ros::Publisher pub3;
ros::Publisher pub4;

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const geometry_msgs::Twist& twist)
{
   pub0.publish(twist);
   pub1.publish(twist);
   pub2.publish(twist);
   pub3.publish(twist);
   pub4.publish(twist);
}

int main(int argc, char **argv)
{ 
   // 初始化ROS节点
   ros::init(argc, argv, "four_subscriber");

   // 创建节点句柄
   ros::NodeHandle n;
   // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
   ros::Subscriber four_sub = n.subscribe("cmd_vel", 10 , poseCallback);

   // 将接收到的消息打印出来
   pub0 = n.advertise<geometry_msgs::Twist>("/tb3_0/cmd_vel", 10);
   pub1 = n.advertise<geometry_msgs::Twist>("/tb3_1/cmd_vel", 10);
   pub2 = n.advertise<geometry_msgs::Twist>("/tb3_2/cmd_vel", 10);
   pub3 = n.advertise<geometry_msgs::Twist>("/tb3_3/cmd_vel", 10);
   pub4 = n.advertise<geometry_msgs::Twist>("/tb3_4/cmd_vel", 10);

   // 循环等待回调函数
   ros::spin();
   return 0;

5）编译功能包

cd ~/catkin_ws
catkin_make

6）仿真运行

##新终端

roslaunch turtlebot3_gazebo multi_turtlebot3.launch

##新终端

rosrun learning_four four_subscriber

##新终端

rosrun turtlebot3_teleop turtlebot3_teleop_key

三、演示视频

图片中5个机器人正在以相同的线速度、角速度进行运动，任务完成。

统一话题控制机器人

四、gazebo环境中搭建场景

1）打开编辑该文件(以已有路径为例)

gedit ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3.launch

2）将所创建的gazebo环境的文件夹如图所示的路径

3）修改如下代码中第二行的world名称，即完成gazebo环境中的场景搭建

 <include file="$(find gazebo_ros)/launch/empty_world.launch">
   <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/test/test.world"/>
   <arg name="paused" value="false"/>
   <arg name="use_sim_time" value="true"/>
   <arg name="gui" value="true"/>
   <arg name="headless" value="false"/>
   <arg name="debug" value="false"/>
 </include>  

4）效果如图