基于gazebo的无人车编队仿真实战总结（二）

基于gazebo的无人车编队仿真实战总结（二）

上一篇博客是通过古月居的多机器人编队仿真考虑讲前期做的基于阿克曼转向的仿真模型，进行三台无人小车的编队仿真
问题 1、
将无人小车的仿真模型，按照博客中gazebo仿真中添加多机器步骤，进行group分组，发现加入group分组后，进行rqt_graph节点显示，能够显示出car1/cmd_vel，但是采用命令
rostopic list ,
查看话题，还是cmd_vel，经过仔细研读前期古月居博客，发现问题，在进行分组命名工作空间的时候，需要区分话题到底是全局变量还是局部变量，改写成cmd_vel之后，话题变为局部变量，此时，就可以根据group的名字进行话题的区分了。
问题2
加入三个无人小车模型后，加载模型的ros_controller相关参数加载不了，一直报错，后来发现是没有在
smart_control_config.yaml文件中的命令空间中，加入" / " 一直都认为写成“car1”就可以了，但是发现，每次都读取不到。

/car1: 
# controls the rear two tires based on individual motors
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
  rear_right_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: rear_right_wheel_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  rear_left_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: rear_left_wheel_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  front_right_steering_position_controller:
    type: effort_controllers/JointPositionController
    joint: front_right_steering_joint
    pid: {p: 40000.0, i: 200.0, d: 1.0}    
  front_left_steering_position_controller:
    type: effort_controllers/JointPositionController
    joint: front_left_steering_joint
    pid: {p: 40000.0, i: 200.0, d: 1.0}

运行命令
roslaunch gazebo_ros emputy_world.launch
roslaunch car_model spawn_car2.launch

rosrun ares_teleop ares_teleop.py
这样就可以实现采用按键控制car1的效果，记住需要source下环境变量

3、问题3
接下来就考虑采用古月居之前的程序，进行三车编队了，运行命令

roslaunch stage_first OnYourMarkGetSetGo_car.launch

  <launch>
    
    <node pkg="stage_first" type="tf_broadcaster"
         args="car2" name="car2_tf_broadcaster">   
</node>
    <node pkg="stage_first" type="tf_broadcaster"
        args="car1" name="car1_tf_broadcaster">        
</node>

    <node pkg="stage_first" type="tf_broadcaster"
          args="car3" name="car3_tf_broadcaster">         
</node>
    <!--node pkg="stage_first" type="master"
          name="master">
         <remap from="/ares1/cmd_vel" to="/car1/cmd_vel" />
         <remap from="/ares1" to="/car1" />
</node-->
    <node pkg="stage_first" type="slave_tf_listener2"
          name="slave2" >
       <remap from="/ares2/cmd_vel" to="/car2/cmd_vel"/>
       <remap from="/ares2" to="/car2" />
    </node>
    <node pkg="stage_first" type="slave_tf_listener3"
          name="slave3" >
        <remap from="/ares3/cmd_vel" to="/car3/cmd_vel" />
        <remap from="/ares3" to="/car3" />
     </node>
  </launch>

发现与之前程序的区别在于stage_fitrst功能包tf_broadcaster.cpp里面订阅的话题，是没有发布出来的，，

ros::Subscriber sub = node.subscribe(robot_name+"/base_pose_ground_truth", 10, &poseCallback);

参考古月系列博客，是需要在xacro车辆模型中加入插件，进行位姿显示

<?xml version="1.0" ?>

<robot name="$(arg roboname)" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="PI" value="3.1415926835897931"/>

    <!-- base -->
  <xacro:property name="base_length" value="1.5"/>
  <xacro:property name="base_width" value="0.8"/>
  <xacro:property name="base_height" value="0.582"/>
  <xacro:property name="base_collision_height" value="0.552"/>
  <xacro:property name="base_mass" value="956"/>
  <xacro:property name="base_mass_ixx" value="343"/>
  <xacro:property name="base_mass_iyy" value="728"/>
  <xacro:property name="base_mass_izz" value="772"/>

  <!-- rear tyre -->
  <xacro:property name="rear_tyre_x" value="0.45"/>
  <xacro:property name="rear_tyre_y" value="0.345"/>
  <xacro:property name="rear_tyre_r" value="0.246"/>
  <xacro:property name="rear_tyre_length" value="0.138"/>
  <xacro:property name="rear_tyre_mass" value="20"/>
  <xacro:property name="rear_tyre_mass_ixx" value="0.5"/>
  <xacro:property name="rear_tyre_mass_iyy" value="0.9"/>
  <xacro:property name="rear_tyre_mass_izz" value="0.5"/> 

  <!-- front tyre -->
  <xacro:property name="front_tyre_x" value="0.452"/>
  <xacro:property name="front_tyre_y" value="0.345"/>
  <xacro:property name="front_tyre_r" value="0.246"/>
  <xacro:property name="front_tyre_length" value="0.138"/>
  <xacro:property name="front_tyre_mass" value="20"/>
  <xacro:property name="front_tyre_mass_ixx" value="0.5"/>
  <xacro:property name="front_tyre_mass_iyy" value="0.9"/>
  <xacro:property name="front_tyre_mass_izz" value="0.5"/>  

  <!-- steering -->
  <xacro:property name="str_angle" value="0.5236"/>    
  <xacro:property name="str_length" value="0.01"/>
  <xacro:property name="str_radius" value="0.138"/>
  <xacro:property name="str_mass" value="5"/>
  <xacro:property name="str_mass_ixx" value="0.012"/>
  <xacro:property name="str_mass_iyy" value="0.025"/>
  <xacro:property name="str_mass_izz" value="0.012"/> 

  <gazebo reference="base_link">
      <material>Gazebo/Green</material>
      <turnGravityOff>false</turnGravityOff>
  </gazebo>

  <!--Car Body-->
  <link name="base_link">
    <collision>
      <origin xyz="0 0 ${base_collision_height}" rpy="0 0 0"/>  
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0 0 ${base_collision_height}" rpy="0 0 0"/>  
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="Green">
            <color rgba="0.16 0.24 0.086 1.0"/>
      </material>
    </visual>
  </link>

  <joint name="inertial_joint" type="fixed">
    <parent link="base_link"/>
    <child link="main_mass"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>  

  <link name="main_mass" type="fixed">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${base_mass}"/>
      <inertia
        ixx="${base_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${base_mass_iyy}"  iyz="0.000000"
        izz="${base_mass_izz}"/>
    </inertial>
  </link>



  <!--Rear Right Wheel-->
  <joint name="rear_right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="rear_right_wheel_link"/>
    <origin xyz="${-rear_tyre_x} ${-rear_tyre_y} ${rear_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="100000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>


  <gazebo reference="rear_right_wheel_link">
      <material>Gazebo/Black</material>
      <turnGravityOff>false</turnGravityOff>
  </gazebo>

  <link name="rear_right_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/>
      </geometry>

      <material name="Black">
            <color rgba="0 0 0 1.0"/>
      </material>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${rear_tyre_mass}"/>
      <inertia
        ixx="${rear_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${rear_tyre_mass_iyy}"  iyz="0.000000"
        izz="${rear_tyre_mass_izz}"/>
    </inertial>
  </link>

  <!--Rear Left Wheel-->
  <joint name="rear_left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="rear_left_wheel_link"/>
    <origin xyz="${-rear_tyre_x} ${rear_tyre_y} ${rear_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="100000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>

  <gazebo reference="rear_left_wheel_link">
      <material>Gazebo/Black</material>
      <turnGravityOff>false</turnGravityOff>
  </gazebo>


  <link name="rear_left_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/>
      </geometry>
      <material name="Black">
            <color rgba="0 0 0 1.0"/>
      </material>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${rear_tyre_mass}"/>
      <inertia
        ixx="${rear_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${rear_tyre_mass_iyy}"  iyz="0.000000"
        izz="${rear_tyre_mass_izz}"/>
    </inertial>
  </link> 

  <!--Front Right Steering-->
  <joint name="front_right_steering_joint" type="revolute">
    <parent link="base_link"/>
    <child link="front_right_steering_link"/>
    <origin xyz="${front_tyre_x} ${-front_tyre_y} ${front_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-str_angle}" upper="${str_angle}" effort="1000000.0" velocity="10000.0"/>
    <dynamics damping="10.0" friction="0.0"/>
  </joint>


 <gazebo reference="front_right_wheel_link">
      <material>Gazebo/Black</material>
      <turnGravityOff>false</turnGravityOff>
  </gazebo>

  <link name="front_right_steering_link">
    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${str_length}" radius="${str_radius}"/>
      </geometry>
      <material name="Black">
            <color rgba="0 0 0 1.0"/>
      </material>

    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${str_mass}"/>
      <inertia
      ixx="${str_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
      iyy="${str_mass_iyy}"  iyz="0.000000"
      izz="${str_mass_izz}"/>
    </inertial>
  </link>


  <!--Front Right Wheel-->
  <joint name="front_right_wheel_joint" type="continuous">
    <parent link="front_right_steering_link"/>
    <child link="front_right_wheel_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="1000000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>




  <gazebo reference="front_right_wheel_link">
      <material>Gazebo/Black</material>
      <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="front_right_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/>
      </geometry> 
      <material name="Black">
            <color rgba="0 0 0 1.0"/>
      </material> 
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${front_tyre_mass}"/>
      <inertia
        ixx="${front_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${front_tyre_mass_iyy}"  iyz="0.000000"
        izz="${front_tyre_mass_izz}"/>
    </inertial>
  </link>

  <!--Front Left Steering-->
  <joint name="front_left_steering_joint" type="revolute">
    <parent link="base_link"/>
    <child link="front_left_steering_link"/>
    <origin xyz="${front_tyre_x} ${front_tyre_y} ${front_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-str_angle}" upper="${str_angle}" effort="1000000.0" velocity="10000.0"/>
    <dynamics damping="10.0" friction="0.0"/>
  </joint>





  <link name="front_left_steering_link">

    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${str_length}" radius="${str_radius}"/>
      </geometry>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${str_mass}"/>
      <inertia
      ixx="${str_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
      iyy="${str_mass_iyy}"  iyz="0.000000"
      izz="${str_mass_izz}"/>
    </inertial>
  </link>


  <!--Front Left Wheel-->
  <joint name="front_left_wheel_joint" type="continuous">
    <parent link="front_left_steering_link"/>
    <child link="front_left_wheel_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="1000000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>




<gazebo reference="front_left_wheel_link">
      <material>Gazebo/Black</material>
      <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="front_left_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/>
      </geometry> 
     <material name="Black">
            <color rgba="0 0 0 1.0"/>
      </material> 
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${front_tyre_mass}"/>
      <inertia
        ixx="${front_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${front_tyre_mass_iyy}"  iyz="0.000000"
        izz="${front_tyre_mass_izz}"/>
    </inertial>
  </link>  



<!-- motors and transmissions for the two rear wheels -->
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rear_right_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>  

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rear_left_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>  

  <!-- EPS and transmissions for the front steering -->
  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="front_right_steering_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="eps_right">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
      <motorTorqueConstant>1000000</motorTorqueConstant>
    </actuator>
  </transmission>

  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="front_left_steering_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="eps_left">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
      <motorTorqueConstant>1000000</motorTorqueConstant>
    </actuator>
  </transmission>



  <!-- Friction Parametres -->

  <gazebo reference="rear_right_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>
  </gazebo>  

  <gazebo reference="rear_left_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>         
  </gazebo>   

  <gazebo reference="front_right_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>         
  </gazebo> 

  <gazebo reference="front_left_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>           
  </gazebo>    

  <!-- Gazebo Plugins -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/$(arg roboname)</robotNamespace>
      <robotParam>robot_description</robotParam>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
      <jointName>rear_left_wheel_joint, rear_right_wheel_joint, front_left_steering_joint, front_right_steering_joint, front_right_wheel_joint, front_left_wheel_joint</jointName>
      <updateRate>50.0</updateRate>
      <robotNamespace>/$(arg roboname)</robotNamespace>
      <alwaysOn>true</alwaysOn>
    </plugin>
  </gazebo>

在此加入插件，可以实时更新车辆的位姿
<gazebo>
    <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
    				<alwaysOn>true</alwaysOn>
    				<updateRate>50.0</updateRate>
    				<bodyName>base_link</bodyName>
   				<topicName>base_pose_ground_truth</topicName>
    				<gaussianNoise>0.01</gaussianNoise>
    				<frameName>world</frameName>
    				<xyzOffsets>0 0 0</xyzOffsets>
   				<rpyOffsets>0 0 0</rpyOffsets>
 			</plugin>
  </gazebo>


</robot>

由此，三车按键控制领航车移动，跟随车2、3进行跟随动作即可实现
效果如下