寫一個組合的機器人硬件接口（ComboRobotHW）

• 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 <iostream>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <pluginlib/class_list_macros.hpp>
#include <ros/ros.h>
//#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 <combo_control/myrobot1_hw.hpp>

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 <iostream>
#include <ros/ros.h>
#include <combined_robot_hw/combined_robot_hw.h>
#include <controller_manager/controller_manager.h>

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

<?xml version="1.0"?>
<package format="2">
  <name>combo_control</name>
  <version>0.0.0</version>
  <description>The combo_control package</description>
  <maintainer email="[email protected]">todo</maintainer>
  <license>TODO</license>
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>controller_manager</build_depend>
  <build_depend>hardware_interface</build_depend>
  <build_depend>myrobot1cpp</build_depend>
  <build_depend>myrobot2cpp</build_depend>
  <depend>combined_robot_hw</depend>
  <build_export_depend>controller_manager</build_export_depend>
  <build_export_depend>hardware_interface</build_export_depend>
  <build_export_depend>myrobot1cpp</build_export_depend>
  <build_export_depend>myrobot2cpp</build_export_depend>
  <exec_depend>controller_manager</exec_depend>
  <exec_depend>hardware_interface</exec_depend>
  <exec_depend>myrobot1cpp</exec_depend>
  <exec_depend>myrobot2cpp</exec_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對於控制器的初始化非常重要。我們只加載一個用於控制軌跡的控制器，但是加載兩個狀態控制器用於給出兩個機器人的狀態。

  參考