翻译自(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 <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="todo@todo.todo">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对于控制器的初始化非常重要。我们只加载一个用于控制轨迹的控制器,但是加载两个状态控制器用于给出两个机器人的状态。