写一个组合的机器人硬件接口（ComboRobotHW）

翻译自(https://github.com/ros-controls/ros_control/wiki/Writing-CombinedRobotHW)

• ComboRobotHW是一个允许将多个RobotHW组合成一个“RobotHW”的软件包。任何控制器现在都看到所有提供的所有RobotHW关节都是一个RobotHW的关节。
• 这背后的机制是特殊的所谓的类加载器（pluginlib）。控制器管理器(controller manager )后面的系统相同，因此ComboRobotHW是某种RobotHW管理器。
• 编写机器人后，可以使用一个控制器管理器(controller manager )来处理整个系统。 但要注意，如果你在不同的RobotHW上有不同的接口（力，速度，位置），你不能只将它们连接到一个控制器上。
• 示例：我们有两个2 DOF机器人。出于某种原因，我们希望将它们链接到一个铰接式4自由度机器人。所以我们先写两个独立的RobotHW。

整体文件结构

launch/
  combo_control.launch
config/
  controllers.yaml
  hardware.yaml
include/
  combo_control/
    myrobot1_hw.hpp
    myrobot2_hw.hpp
src/
    myrobot1_hw.cpp
    myrobot2_hw.cpp
    combo_control_node.cpp
CMakeLists.txt
package.xml
myrobots_hw_plugin.xml

myrobot1_hw.hpp

#include 
#include 
#include 
#include 
#include 
#include 
//#include "myrobot1cpp/myrobot1cpp.hpp"

namespace myrobots_hardware_interface
{

class MyRobot1Interface: public hardware_interface::RobotHW
{
public:
    MyRobot1Interface();
    ~MyRobot1Interface();
    bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh);
    void read(const ros::Time& time, const ros::Duration& period);
    void write(const ros::Time& time, const ros::Duration& period);

protected:
    ros::NodeHandle nh_;
    
    //interfaces
    hardware_interface::JointStateInterface joint_state_interface;
    hardware_interface::EffortJointInterface effort_joint_interface;

    int num_joints;
    std::vector<string> joint_name;

    //actual states
    std::vector<double> joint_position_state;
    std::vector<double> joint_velocity_state;
    std::vector<double> joint_effort_state;

    //given setpoints
    std::vector<double> joint_effort_command;

    //MyRobot1CPP* robot;
};
}

myrobot1_hw.cpp

#include 

namespace myrobots_hardware_interface
{

MyRobot1Interface::MyRobot1Interface(){

}

MyRobot1Interface::~MyRobot1Interface(){

}

bool MyRobot1Interface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh){
    //init base
    //robot = myrobot1cpp::initRobot();

    //get joint names and num of joint
    robot_hw_nh.getParam("joints", joint_name);
    num_joints = joint_name.size();

    //resize vectors
    joint_position_state.resize(num_joints);
    joint_velocity_state.resize(num_joints);
    joint_effort_state.resize(num_joints);
    joint_effort_command.resize(num_joints);
 
    //Register handles
    for(int i=0; i<num_joints; i++){
        //State
        JointStateHandle jointStateHandle(joint_name[i], &joint_position_state[i], &joint_velocity_state[i], &joint_effort_state[i]);
        joint_state_interface.registerHandle(jointStateHandle);
        

        //Effort
        JointHandle jointEffortHandle(jointStateHandle, &joint_effort_command[i]);
        effort_joint_interface.registerHandle(jointEffortHandle);
    }

    //Register interfaces
    registerInterface(&joint_state_interface);
    registerInterface(&effort_joint_interface);
    
    //return true for successful init or ComboRobotHW initialisation will fail
    return true;
}

void MyRobot1Interface::read(const ros::Time& time, const ros::Duration& period){
        for(int i=0;i < num_joints;i++){
            //joint_position_state[i] = robot.readJoints(i);
        }
}

void MyRobot1Interface::write(const ros::Time& time, const ros::Duration& period){
   for(int i=0;i < num_joints;i++){
       //robot.writeJoints(joint_effort_command[i]);
   }
}

}
PLUGINLIB_EXPORT_CLASS(myrobots_hardware_interface::MyRobot1Interface, hardware_interface::RobotHW)

• 对于myrobot2_hw.hpp和.cpp也是如此，只是"1"改成“2”。 注意，PLUGINLIB_EXPORT_CLASS位于命名空间之外。

combo_control_node.cpp

#include 
#include 
#include 
#include 

int main(int argc, char** argv){
    ros::init(argc, argv, "combo_control_node");
   
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle nh;
    combined_robot_hw::CombinedRobotHW hw;
    bool init_success = hw.init(nh,nh);

    controller_manager::ControllerManager cm(&hw,nh);

    ros::Duration period(1.0/200); // 200Hz update rate

    ROS_INFO("combo_control started");
    while(ros::ok()){
        hw.read(ros::Time::now(), period);
        cm.update(ros::Time::now(), period);
        hw.write(ros::Time::now(), period);
        period.sleep();
    }

    spinner.stop();
return 0;
}

• CombinedRobotHW将完成所有从参数服务器加载参数以及初始化和启动所有控制器和硬件的工作。请注意，我们使用一个控制器管理器(controller manage)来处理两个RobotHW。

hardware.yaml

robot_hardware:
  - myrobot1_hw
  - myrobot2_hw
myrobot1_hw:
  type: myrobots_hardware_interface/MyRobot1Interface
  joints:
    - mr1_joint1
    - mr1_joint2
myrobot2_hw:
  type: myrobots_hardware_interface/MyRobot2Interface
  joints:
    - mr2_joint1
    - mr2_joint2

controllers.yaml

comboRobot:
    myrobot1:
      hardware_interface:
        joints:
          - mr1_joint1
          - mr1_joint2
    myrobot2:
      hardware_interface:
        joints:
          - mr2_joint1
          - mr2_joint2
    controller:
      myrobot1_state:
        type: joint_state_controller/JointStateController
        publish_rate: 200
      myrobot2_state:
        type: joint_state_controller/JointStateController
        publish_rate: 200

      combo_trajectory:
        type: effort_controllers/JointTrajectoryController
        joints:
          - mr1_joint1
          - mr1_joint2
          - mr2_joint1
          - mr2_joint2
        gains:
          mr1_joint1: {p: 30.0, i: 1, d: 0, i_clamp_min: -1, i_clamp_max: 1}
          mr1_joint2: {p: 30.0, i: 1, d: 0, i_clamp_min: -1, i_clamp_max: 1}
          mr2_joint1: {p: 30.0, i: 1, d: 0, i_clamp_min: -1, i_clamp_max: 1}
          mr2_joint2: {p: 30.0, i: 1, d: 0, i_clamp_min: -1, i_clamp_max: 1}
        velocity_ff:
          mr1_joint1: 1.0
          mr1_joint2: 1.0
          mr2_joint1: 1.0
          mr2_joint2: 1.0
        constrains:
          goal_time: 10.0
          mr1_joint1:
            goal: 0.5
          mr1_joint2:
            goal: 0.5
          mr2_joint1:
            goal: 0.5
          mr2_joint2:
            goal: 0.5

• 注意，两个设备现在都在一个控制器controller中。关节名称必须与urdf中的名称相同。

myrobots_hw_plugin.xml

<library path="lib/libmyrobots_hardware_interfaces">
  <class name="myrobots_hardware_interface/MyRobot1Interface" type="myrobots_hardware_interface::MyRobot1Interface" base_class_type="hardware_interface::RobotHW">
    <description>
      Interface for MyRobot1
    description>
  class>
  <class name="myrobots_hardware_interface/MyRobot2Interface" type="myrobots_hardware_interface::MyRobot2Interface" base_class_type="hardware_interface::RobotHW">
    <description>
      Interface for MyRobot2
    description>
  class>
library>

• 这个文件将myrobot_hw类注册为combo_control包的插件。

package.xml

<package format="2">
  <name>combo_controlname>
  <version>0.0.0version>
  <description>The combo_control packagedescription>
  <maintainer email="[email protected]">todomaintainer>
  <license>TODOlicense>
  <buildtool_depend>catkinbuildtool_depend>
  <build_depend>controller_managerbuild_depend>
  <build_depend>hardware_interfacebuild_depend>
  <build_depend>myrobot1cppbuild_depend>
  <build_depend>myrobot2cppbuild_depend>
  <depend>combined_robot_hwdepend>
  <build_export_depend>controller_managerbuild_export_depend>
  <build_export_depend>hardware_interfacebuild_export_depend>
  <build_export_depend>myrobot1cppbuild_export_depend>
  <build_export_depend>myrobot2cppbuild_export_depend>
  <exec_depend>controller_managerexec_depend>
  <exec_depend>hardware_interfaceexec_depend>
  <exec_depend>myrobot1cppexec_depend>
  <exec_depend>myrobot2cppexec_depend>

  <export>
    <hardware_interface plugin="${prefix}/myrobots_hw_plugin.xml"/>
  export>
package>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(combo_control)

find_package(catkin REQUIRED COMPONENTS
  controller_manager
  hardware_interface
  myrobot1cpp
  myrobot2cpp
  combined_robot_hw
)

catkin_package(
  INCLUDE_DIRS include
  CATKIN_DEPENDS controller_manager hardware_interface myrobot1cpp myrobot2cpp combined_robot_hw
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_library(myrobots_hardware_interfaces 
    src/myrobot1_hw.cpp
    src/myrobot2_hw.cpp
)
target_link_libraries(myrobots_hardware_interfaces ${catkin_LIBRARIES})

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

• 请注意，我们创建了myrobots_hw_plugin.xml中声明的库myrobots_hardware_interfaces。

combo_control.launch

<launch>
<rosparam file="$(find combo_control)/config/controllers.yaml" command="load"/>
    <rosparam file="$(find combo_control)/config/hardware.yaml" command="load"/>

    <param name="robot_description" textfile="$(find robot_description)/robots/robot.urdf"/>

    <node name="combo_control_node" pkg="combo_control" type="combo_control_node" output="screen"/>
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/"
        args="
             /comboRobot/controller/myrobot1_state
             /comboRobot/controller/myrobot2_state
             /comboRobot/controller/combo_trajectory 
    "/>
launch>

• 注意robot.urdf对于控制器的初始化非常重要。我们只加载一个用于控制轨迹的控制器，但是加载两个状态控制器用于给出两个机器人的状态。