Writing CombinedRobotHW

ComboRobotHW is a package that allows multiple RobotHWs be combined into one "RobotHW". Any controller see now all provided joints of all RobotHWs as joints of one RobotHW.

The mechanism behind this is special so called class loader (pluginlib). Same system behind controller manager, so ComboRobotHW is some kind of RobotHW Manager.

After you composed your robot you can use one controller manager for the entire system. But be aware that if you have different interfaces (effort, velocity, position) on different RobotHWs, you can't just connect them to one for example effort controller.

Example: We have two 2 DOF robots. For some reason we want to chain them to one articulated 4 DOF robot. So we first write 2 separate RobotHWs.

File structure of package combo_control:

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

Content of 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;
};
}

Content of 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)

Same for myrobot2_hw.hpp and .cpp but with number "2" everywhere. Note that init is returning bool, if init will return false then combohw will fail to initialise. Note that init, read and write has parameters, they are strictly and has to be written like this. Note that constructor and destructor are parameterless. Note that PLUGINLIB_EXPORT_CLASS is outside of namespace.

Content of 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::Rate rate(200); // 200Hz update rate

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

    spinner.stop();
return 0;
}

CombinedRobotHW will do all the work for loading parameters from parameter server and initialisating and starting all controllers and hardware. Note that we use one controller manager for handling two RobotHWs.

Content of 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

This file descripces of what hardware the robot is made. Note that joints from different devices are named differently. The joint names has to be the same as in urdf.

Content of 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

Note that both devices are now in one controller. Joint names has to be the same as in urdf.

Content of 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>

This file register our myrobot_hw classes as plugins of packages combo_control.

Content of 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>

Note the export tag and his contents.

Content of 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})

Note that we create library myrobots_hardware_interfaces that is mentioned in myrobots_hw_plugin.xml

Content of 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 is important for initialisation of controllers Note that we are loading only one controller for controlling trajectory but two state controllers for giving the state of both robots.