-
Notifications
You must be signed in to change notification settings - Fork 305
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 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::Duration period(1.0/200);
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 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.
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.
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.