gazebo教程---ros_control

一. ros_control和Gazebo的数据流向
在Gazebo中模拟机器人的控制器是可以通过使用ros_control和一个简单的Gazebo插件适配器来完成。下面是仿真，硬件，控制器和传动之间关系的概览：

二. 使用
（1）向URDF中添加传动元素
为了使用ros_control，需要给URDF中添加一些其它元素，比如< transmission>，它是为了链接执行器和关节。而在< transmission>标签中一些重要子标签有：

• < joint name=" ">：name对应URDF中的joint的name
• < type>：传动的类型，目前只有"transmission_interface/SimpleTransmission" 实现
• < hardwareInterface>：此标签存在于< actuator>和< joint>标签，它会告诉gazebo_ros_control插件要加载的硬件接口是什么（position，velocity or effort interfaces），目前只实现effort interfaces。

一些可用的硬件接口：

• Joint Command Interface - Hardware interface to support commanding an array of joints. Note that these commands can have any semantic meaning as long as they each can be represented by a single double, they are not necessarily effort commands. To specify a meaning to this command, see the derived classes:
  Effort Joint Interface - for commanding effort-based joints.
  Velocity Joint Interface - for commanding velocity-based joints.
  Position Joint Interface - for commanding position-based joints.
• Joint State Interfaces - Hardware interface to support reading the state of an array of named joints, each of which has some position, velocity, and effort (force or torque).
• Actuator State Interfaces - Hardware interface to support reading the state of an array of named actuators, each of which has some position, velocity, and effort (force or torque).
• Actuator Command Interfaces:
  Effort Actuator Interface
  Velocity Actuator Interface
  Position Actuator Interface
• Force-torque sensor Interface
• IMU sensor Interface

（2）添加gazebo_ros_control插件
除了transmission标签，也需要向你的URDF中添加Gazebo插件，它会自动解析transmission标签，装载合适的硬件接口和controller manager。一般默认的gazebo_ros_control插件是很简单的，但是它也是可以通过插件架构在ros_control和gazebo之间来扩展自定义的机器人硬件接口。
一般添加到URDF的默认插件形如：

  "gazebo_ros_control" filename="libgazebo_ros_control.so">
    /MYROBOT</robotNamespace>
  </plugin>
</gazebo>

这个 gazebo_ros_control的< plugin>标签有如下可选的子标签：

• < robotNamespace>: 插件实例所使用的ROS命名空间，默认为URDF中robot的name
• < controlPeriod>: 控制器的更新时钟，默认为gazebo的时钟
• < robotParam>: 参数服务器中robot_description的位置，默认为’/robot_description’
• < robotSimType>:所使用自定义机器人仿真接口的插件名字，默认为’DefaultRobotHWSim’

自定义gazebo_ros_control仿真插件：
这个gazebo_ros_controlGazebo插件为仿真更加复杂机理提供了基于插件的接口来实现自定义Gazebo和ros_control之间的接口。这些插件必须继承gazebo_ros_control::RobotHWSim，它实现一个模拟的ros_control的hardware_interface::RobotHW。RobotHWSim为对Gazebo仿真器的读取和控制关节属性提供API级别的访问。
相应的RobotHWSim子类在URDF模型中指定，并且在机器人模型装载时装载。例如，下面的XML会装载默认插件：

  "gazebo_ros_control" filename="libgazebo_ros_control.so">
    /MYROBOT</robotNamespace>
    gazebo_ros_control/DefaultRobotHWSim</robotSimType>
  </plugin>
</gazebo>

三. 例子
（1）首先在rrbot.xacro中添加< transmission>标签，同时< hardwareInterface>必须包含在< joint>和< actuator>标签中，内容如下：

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

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

（2）在rrbot.gazebo中添加ros_control插件
内容如下：

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

（3）创建.yaml配置文件

cd ~/catkin_ws/src
catkin_create_pkg rrbot_control controller_manager joint_state_controller robot_state_publisher
cd rrbot_control
mkdir config
mkdir launch

在config文件下创建一个文件rrbot_control.yaml，内容如下：

rrbot:
  # 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: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

保存在yaml的PID增益和控制器设置可以通过launch文件加载到参数服务器中，因此在launch文件夹创建一个rrbot_control.launch文件，内容如下：

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

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

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

</launch>

< rosparam>加载控制器配置文件到参数服务器中；节点controller_spawner通过运行一个python脚本为rrbot启动两个关节位置控制器，这个python脚本会向ros_control控制器管理者（controller manager）请求服务调用，这个服务调用会告诉controller manager你想要的是哪个控制器。同时，这个节点也加载了第三个控制器joint_state_controller，用来发布所有关节的关节状态，相应话题为“/joint_states“。
最后是启动robot_state_publisher节点，接收来自joint_state_controller的话题是"/joint_states"，发布的话题是“/tf"。
（4）使用roslaunch启动控制器
首先启动rrbot仿真：

roslaunch rrbot_gazebo rrbot_world.launch

然后给两个关节加载控制器：

roslaunch rrbot_control rrbot_control.launch

• 手动给控制器发布命令

rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"

• 使用RQT发送命令

rosrun rqt_gui rqt_gui

在RQT的菜单栏中“Plugins”中点击"Topics->Message Publisher"插件，然后从Topic的下拉框中选择你想要给控制器发布的话题，比如/rrbot/joint1_position_controller/command，设置结果如下（其中i是RQT的时间变量）：

此时Gazebo中的rrbot开始做周期运动：

在RQT的菜单栏中Plugins->Visualization点击“Plot”，订阅话题“/rrbot/joint1_position_controller/command/data”可以显示发给控制器/rrbot/joint1_position_controller的数据，添加订阅另一个话题“/rrbot/joint1_position_controller/state/process_value”可以显示实际关节运动的状态值，效果如下图：

通过调节PID参数可以减小关节跟踪误差，而调节PID参数可以通过Plugins->Configuration中的Dynamic Reconfigure插件，如下图：

（4）利用Riz显示

rosrun rviz rviz

刚打开rviz时会报错，如下图：

解决方法：将Global Options的Fixed Frame修改为world，就可以在rviz中显示Gazebo里的模型运动状态。

节点图如下：

参考：
http://gazebosim.org/tutorials/?tut=ros_control