在gazebo中对机器人进行控制，并在rviz中同步显示

在上一篇博客中，我一步一步地建立了在gazebo仿真中能用的xacro文件。但是仿真时的模型是自由摆动的，文末的时候我想对他进行控制，但是篇幅太长，所以新开一篇。
参考ros_control的内容：http://blog.csdn.net/wxflamy/article/details/79228736

先列出xacro文件：

<?xml version="1.0"?>
<robot name="myfirstrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Constants for robot dimensions -->
  <xacro:property name="PI" value="3.1415926535897931"/>
  <xacro:property name="mass" value="1" /> <!-- arbitrary value for mass -->
  <xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams -->
  <xacro:property name="height1" value="4" /> <!-- Link 1 -->
  <xacro:property name="height2" value="2" /> <!-- Link 2 -->
  <xacro:property name="height3" value="1" /> <!-- Link 3 -->

  <!-- ros_control plugin -->
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/myfirstrobot</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
        </plugin>
    </gazebo>

  <!-- Link1 -->
  <gazebo reference="base_link">
    <material>Gazebo/Orange</material>
  </gazebo>

  <!-- Link2 -->
  <gazebo reference="middle_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Blue</material>
  </gazebo>

  <!-- Link3 -->
  <gazebo reference="top_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Orange</material>
  </gazebo>

    <!-- Import Rviz colors -->
  <xacro:include filename="$(find myurdf)/robot/materials.xacro" />

  <!-- Used for fixing robot to Gazebo 'base_link' -->
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>

    <!-- Base Link -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="${width} ${width} ${height1}"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 ${height1/2}"/>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <cylinder length="${height1}" radius="${width+0.1}"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 ${height1/2}"/>
        </collision>
        <inertial>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <mass value="${mass}"/>
      <inertia
                ixx="${mass / 12.0 * (width*width + width*width)}" ixy="0.0" ixz="0.0"
            iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
            izz="${mass / 12.0 * (width*width + height1*height1)}"/>
        </inertial>
    </link>

    <joint name="joint1" type="continuous">
        <parent link="base_link"/>
        <child link="middle_link"/>
        <origin rpy="0 0 0" xyz="0 0 ${height1}"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="middle_link">
        <visual>
            <geometry>
                <box size="${width} ${width} ${height2}"/>
            </geometry>
            <origin rpy="0 ${PI/2} 0" xyz="${height2/2} 0 0"/>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <cylinder length="${height2}" radius="${width+0.1}"/>
            </geometry>
            <origin rpy="0 ${PI/2} 0" xyz="${height2/2} 0 0"/>
        </collision>
        <inertial>
      <origin xyz="${height2/2} 0 0" rpy="0 0 0"/>
      <mass value="${mass}"/>
      <inertia
                ixx="${mass / 12.0 * (width*width + height2*height2)}" ixy="0.0" ixz="0.0"
            iyy="${mass / 12.0 * (height2*height2 + width*width)}" iyz="0.0"
            izz="${mass / 12.0 * (width*width + width*width)}"/>       
        </inertial>
    </link>

    <joint name="joint2" type="continuous">
        <parent link="middle_link"/>
        <child link="top_link"/>
        <origin rpy="0 0 0" xyz="${height2} 0 0"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="top_link">
        <visual>
            <geometry>
                <box size="${width} ${width} ${height3}"/>
            </geometry>
            <origin rpy="0 ${PI/2} 0" xyz="${height3/2} 0 0"/>
            <material name="orange"/>
        </visual>
        <collision>
            <geometry>
                <cylinder length="${height3}" radius="${width+0.1}"/>
            </geometry>
            <origin rpy="0 ${PI/2} 0" xyz="${height3/2} 0 0"/>
        </collision>
        <inertial>
      <origin xyz="${height3/2} 0 0" rpy="0 0 0"/>
      <mass value="${mass}"/>
      <inertia
                ixx="${mass / 12.0 * (width*width + height3*height3)}" ixy="0.0" ixz="0.0"
            iyy="${mass / 12.0 * (height3*height3 + width*width)}" iyz="0.0"
            izz="${mass / 12.0 * (width*width + width*width)}"/>
        </inertial>
    </link>

    <joint name="end" type="revolute">
        <axis xyz="1 0 0"/>
        <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
        <parent link="top_link"/>
        <child link="end_link"/>
        <origin rpy="0 0 0" xyz="${height3} 0 0"/>
    </joint>

    <link name="end_link">
        <visual>
            <geometry>
                <sphere radius="${width}"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${width+0.05}"/>
            </geometry>
        </collision>
        <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${mass}"/>
      <inertia
                ixx="${mass / 12.0 * (width*width + width*width)}" ixy="0.0" ixz="0.0"
            iyy="${mass / 12.0 * (width*width + width*width)}" iyz="0.0"
            izz="${mass / 12.0 * (width*width + width*width)}"/>
        </inertial>
    </link>

  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

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

  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="end">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</robot>

上一节的启动文件里没有包含控制信息，现在我们配置一下控制器。新建一个config文件夹保存控制器参数。新建robot_control.yaml文件：

myfirstrobot:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 20.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 20.0, i: 0.01, d: 10.0}

为joint1和joint2配置了位置控制器。
然后在启动文件中增加控制器相关的启动文件，完整的启动文件为：

<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <!--arg name="world_name" value="$(find myurdf)/robot/robot.world"/-->
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
    command="$(find xacro)/xacro --inorder '$(find myurdf)/robot/myfirstrobot.xacro'" />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model myfirstrobot -param robot_description"/>

  <node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen"
        args="-d $(find myurdf)/launch/xacro.rviz" />

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find myurdf)/config/robot_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/myfirstrobot" args="joint_state_controller
                      joint1_position_controller
                      joint2_position_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/myfirstrobot/joint_states" />
  </node>
</launch>

控制参数调的不是很好，打开的时候可能会出现一段时间的摆动，然后停止。需要调PID参数，这里我就不调了。

以下命令可以向关节发送控制位置命令

rostopic pub -1 /myfirstrobot/joint1_position_controller/command std_msgs/Float64 "data: 0"
rostopic pub -1 /myfirstrobot/joint2_position_controller/command std_msgs/Float64 "data: 1"

以下命令可以在终端打印出机器人的运行状态

rostopic echo myfirstrobot/joint_states