<ROS> Gazebo Ros Control 及 Controller运用

1. 写在前面

第一篇 – 机器人描述: http://blog.csdn.net/sunbibei/article/details/52297524
第二篇 – TRANSMISSION: http://blog.csdn.net/sunbibei/article/details/53287975

在我上一篇博客上, 我们引入了UR5BH机器人， 另外， 对其描述文件中的transmission进行了解析， 其中摘录了如下文件：

<gazebo>
  <plugin filename="libgazebo_ros_control.so" name="ros_control">
    <!--robotNamespace>/</robotNamespace-->
    <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
  </plugin>
</gazebo>
... ...
<transmission name="shoulder_pan_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="shoulder_pan_joint">
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="shoulder_pan_motor">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>
... ...
<transmission name="bh_j32_transmission">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="bh_j32_joint"/>
  <actuator name="bh_j32">
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
    <motorTorqueConstant>1</motorTorqueConstant>
  </actuator>
</transmission>

在这段描述中， 除了transmission之外， 另一个引入的， 就是gazebo_ros_control. 本篇作为第三篇， 主要和大家分享一些关于gazebo_ros_control相关的内容。

2. Gazebo ROS Control

在前一篇的基础上， 实际上我们已经能够开始配置MoveIt！来进行机器人控制了。 但是， 为了更进一步的让事情更加清晰， 我们还是耐下心再探讨一下gazebo_ros_control。 包布局如下图所示:

打开package.xml文件， 除了一些依赖包的定义外， 一个比较关键的地方是在export块中， 摘录如下：

... ...
<export>
    <gazebo_ros_control plugin="${prefix}/robot_hw_sim_plugins.xml"/>
</export>
... ...

了解pluginlib的同学肯定知道， 这是将plugin的定义文件导入的步骤。 不熟悉的同学， 可以参考1, 或者 参考2 进行了解。

导入的文件在文件系统中可以看到， 摘录如下：

<library path="lib/libdefault_robot_hw_sim">
  <class
    name="gazebo_ros_control/DefaultRobotHWSim"
    type="gazebo_ros_control::DefaultRobotHWSim"
    base_class_type="gazebo_ros_control::RobotHWSim">
    <description>
      A default robot simulation interface which constructs joint handles from an SDF/URDF.
    </description>
  </class>
</library>

可以看到， 该文件中定义了一个plugin， 就是gazebo_ros_control::DefaultRobotHWSim, 该plugin可以动态的被加载进来。再查看该包的CMakeLists.txt, 可以看到下述内容：

## 2.1. Libraries
add_library(${PROJECT_NAME} src/gazebo_ros_control_plugin.cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

add_library(default_robot_hw_sim src/default_robot_hw_sim.cpp)
target_link_libraries(default_robot_hw_sim ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

该包总共会生成两个库， 第一个库是gazebo_ros_control， 第二个库是default_robot_hw_sim, 分别都只有一个文件。 下面我们分别来分析这两个库的源码以及所对应完成的任务。

3. Default Robot HW SIM

在default_robot_hw_sim.cpp中， 定义了类DefaultRobotHWSim， 该类继承于gazebo_ros_control::RobotHWSim, 父类定义摘录如下：

namespace gazebo_ros_control {

  // Struct for passing loaded joint data
  struct JointData 
  {
    std::string name_;
    std::string hardware_interface_;

    JointData(const std::string& name, const std::string& hardware_interface) :
      name_(name),
      hardware_interface_(hardware_interface)
    {}
  };

  class RobotHWSim : public hardware_interface::RobotHW 
  {
  public:
    virtual ~RobotHWSim() { }
    virtual bool initSim(
        const std::string& robot_namespace,
        ros::NodeHandle model_nh, 
        gazebo::physics::ModelPtr parent_model,
        const urdf::Model *const urdf_model, // 留意该指针有两个const
        // TransmissionInfo的内容在前一篇博客中有介绍
        std::vector<transmission_interface::TransmissionInfo> transmissions) = 0;
    virtual void readSim(ros::Time time, ros::Duration period) = 0;
    virtual void writeSim(ros::Time time, ros::Duration period) = 0;

    virtual void eStopActive(const bool active) {}
  };
}

该类定义了gazebo plugin版本的RobotHW，RobotHW定义于ros control包中， 定义了与实际机器人通讯的基本接口， 在编写实际机器人的ros control体系时， 该类是用户必须实现的类。 主要重载read(), write(), preparewitch() 以及doSwitch()四个接口。 在RobotHW中， 这四个函数都有默认实现（默认实现是空， 什么都不做）， 都不是纯虚函数。 一般而言， 重载读写函数即可正常工作了。 这个地方， 只是将接口换了个名字， 并且， 几个纯虚函数都必须在子类中进行实现。

类DefaultRobotHWSim的定义摘录如下：

namespace gazebo_ros_control
{

class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
{
public:

  // 四个纯需函数必须在子类中进行实现。
  virtual bool initSim(
    const std