ros_control从入门到放弃：写一个机械臂的硬件接口，并与moveit进行对接控制（附机械臂例子）
在创建自己的机械臂的时候需要一个动作服务器controller和RobotHW与moveit进行对接，如下图，而幸运的是这些都已经被写好了框架，下面就来看看原理与实现吧。
参考：
http://wiki.ros.org/ros_control
https://github.com/ros-controls/ros_control/wiki
hardware_interface::RobotHW Class Reference
先上几张图有下整体印象。
下面是一些常用的控制器接口。
一个最简单的控制环就如下图这样：读取硬件状态－控制器管理器更新－写命令到硬件
下面开始写一个标准接口。
假设我们有一个带有2个关节的机器人：关节A和B。两个关节都是位置控制模式。 在这种情况下，你的机器人应该提供标准的JointPositionInterface和JointStateInterface，这样它就可以重用所有已编写的控制器来使用JointPositionInterface和JointStateInterface。
MyRobot类:
#include 那么这段代码如何实际控制你的机器人呢？
- 上述功能旨在使控制器管理器controller manager（以及控制器管理器内的控制器）能够访问机器人的关节状态以及机器人的命令。 当控制器管理器运行时，控制器将读取机器人中的
pos，vel和eff变量，并将所需的命令写入cmd变量。 - 确保
pos，vel和eff变量始终具有最新的关节状态joint states是你的工作，你还需要确保写入cmd变量的内容由机器人执行。 为此，你可以向机器人类添加read()和write()函数。 在你的main()中可以这样写：
main()
{
MyRobot robot;
controller_manager::ControllerManager cm(&robot);
while (true)
{
robot.read();
cm.update(robot.get_time(), robot.get_period());
robot.write();
sleep();
}
}
如上图所示，我们不仅限于从一个单一接口继承。 机器人可以提供任意数量的接口。 例如，你的机器人可以提供PositionJointInterface和VelocityJointInterface等等。
举个例子
写一个控制器，通过roslaunch启动。先看效果，后面上全套代码，可以直接复制粘贴使用。
可以看到话题如下：
可以通过rqt中的Joint trajectory controller控制
通过配置moveit中的controllers.yaml保持话题名一致来实现moveit对控制器的控制：
controller_list:
- name: /dh_arm_controller
action_ns: "follow_joint_trajectory"
type: FollowJointTrajectory
default: true
joints:
- arm_joint_1
- arm_joint_2
- arm_joint_3
- arm_joint_4
joints:
- arm_joint_1
- arm_joint_2
- arm_joint_3
- arm_joint_4
规划运行时发送的数据效果：
-
文件树：
├── CMakeLists.txt
├── config
│ └── dh_arm_control.yaml
├── include
│ └── dhrobot_controller
│ └── arm_hardware_interface.h
├── launch
│ └── arm_ros_control.launch
├── package.xml
└── src
└── arm_hardware_interface.cpp -
dh_arm_control.yaml
dh_arm_controller:
type: position_controllers/JointTrajectoryController
joints:
- arm_joint_1
- arm_joint_2
- arm_joint_3
- arm_joint_4
constraints:
goal_time: 0.5
stopped_velocity_tolerance: 0.2
stop_trajectory_duration: 0
state_publish_rate: 20
action_monitor_rate: 10
- arm_hardware_interface.h
#ifndef ARM_HW_INTERFACE
#define ARM_HW_INTERFACE
#include - arm_ros_control.launch
<launch>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find dhrobot_description)/urdf/dhrobot.urdf.xacro'" />
<node name="arm_hardware_interface" pkg="dhrobot_controller" type="arm_hardware_interface" output="screen"/>
<rosparam file="$(find dhrobot_controller)/config/dh_arm_control.yaml" command="load"/>
<node name="dh_arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn dh_arm_controller" respawn="false" output="screen"/>
launch>
- arm_hardware_interface.cpp
#include "dhrobot_controller/arm_hardware_interface.h"
boost::mutex Arm_io_mutex;
ArmHWInterface::ArmHWInterface()
{
for(int i=0; i<4; i++)
{
std::string joint_cmd_name="arm_joint_";
std::string joint_state_name="arm_joint_";
std::string joint_num=boost::lexical_cast<std::string>(i+1);
joint_cmd_name.append(joint_num);
joint_state_name.append(joint_num);
joint_cmd_name.append("_controller/command");
joint_state_name.append("_controller/state");
pub_cmd[i]=_n.advertise<std_msgs::Float64>(joint_cmd_name, 1);
sub_js[i]=_n.subscribe(joint_state_name, 1, &ArmHWInterface::read, this);
client_controller_list=_n.serviceClient<controller_manager_msgs::ListControllers>("/controller_manager/list_controllers");
loaded_flag=false;
}
for(int i=0; i<4; i++)
{
std::string joint_name="arm_joint_";
std::string joint_num=boost::lexical_cast<std::string>(i+1);
joint_name.append(joint_num);
hardware_interface::JointStateHandle jnt_state_handle_tmp(joint_name, &pos[i], &vel[i], &eff[i]);
jnt_state_interface.registerHandle(jnt_state_handle_tmp);
}
registerInterface(&jnt_state_interface);
for(int i=0; i<4; i++)
{
std::string joint_name="arm_joint_";
std::string joint_num=boost::lexical_cast<std::string>(i+1);
joint_name.append(joint_num);
hardware_interface::JointHandle jnt_handle_tmp(jnt_state_interface.getHandle(joint_name), &cmd[i]);
jnt_cmd_interface.registerHandle(jnt_handle_tmp);
}
registerInterface(&jnt_cmd_interface);
start_time_=ros::Time::now();
start_dur_.operator +=(ros::Duration(3));
}
void ArmHWInterface::publishCmd()
{
if(ros::Time::now()-start_time_<start_dur_)
return;
for(int i=0; i<4; i++)
{
cmd_msg[i].data=cmd[i];
pub_cmd[i].publish(cmd_msg[i]);
}
}
void ArmHWInterface::read(const dynamixel_msgs::JointStateConstPtr &msg)
{
if(msg->motor_ids.size()<=0)
{
return;
}
if(msg->motor_ids[0]>6 || msg->motor_ids[0]<0)
{
return;
}
int msg_num=msg->motor_ids[0];
double bm=msg->current_pos-pos[msg_num];
boost::mutex::scoped_lock lock(Arm_io_mutex);
pos[msg_num]=msg->current_pos;
if(ros::Time::now()-start_time_>start_dur_)
{
if(bm>=0)
vel[msg_num]=msg->velocity;
else
vel[msg_num]=-1*msg->velocity;
}
else
vel[msg_num]=0;
if(fabs(vel[msg_num])<1.2)
vel[msg_num]=0;
eff[msg_num]=msg->load;
}
ros::NodeHandle ArmHWInterface::getnode()
{
return _n;
}
static void timespecInc(struct timespec &tick, int nsec)
{
int SEC_2_NSEC = 1e+9;
tick.tv_nsec += nsec;
while (tick.tv_nsec >= SEC_2_NSEC)
{
tick.tv_nsec -= SEC_2_NSEC;
++tick.tv_sec;
}
}
void* update_loop(void* threadarg)
{
controller_manager::ControllerManager *c=(controller_manager::ControllerManager *)threadarg;
ros::Rate r(50);
ros::Duration d(0.02);
while(ros::ok())
{
boost::mutex::scoped_lock lock(Arm_io_mutex);
c->update(ros::Time::now(), d);
lock.unlock();
r.sleep();
}
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"Dhrobot Arm_hardware_interface", ros::init_options::AnonymousName);
ArmHWInterface c1;
controller_manager::ControllerManager cm(&c1);
pthread_t tid;
pthread_create(&tid, NULL, update_loop, (void *)&cm);
ROS_INFO("Arm bring up successfully");
// loop
ros::Rate r(50);
while(ros::ok())
{
boost::mutex::scoped_lock lock(Arm_io_mutex);
c1.publishCmd();
lock.unlock();
ros::spinOnce();
r.sleep();
}
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(dhrobot_controller)
find_package(catkin REQUIRED COMPONENTS
actionlib
urdf
cmake_modules
control_msgs
control_toolbox
controller_manager
hardware_interface
joint_limits_interface
roscpp
sensor_msgs
std_msgs
trajectory_msgs
transmission_interface
)
catkin_package(
)
include_directories(
include/
${catkin_INCLUDE_DIRS}
)
add_executable(arm_hardware_interface src/arm_hardware_interface.cpp)
target_link_libraries(arm_hardware_interface ${catkin_LIBRARIES})
package.xml
<package>
<name>dhrobot_controllername>
<version>1.0.0version>
<description>The dhrobot_controller packagedescription>
<maintainer email="[email protected]">wxwmaintainer>
<license>BSDlicense>
<buildtool_depend>catkinbuildtool_depend>
<build_depend>actionlibbuild_depend>
<build_depend>urdfbuild_depend>
<build_depend>cmake_modulesbuild_depend>
<build_depend>control_msgsbuild_depend>
<build_depend>control_toolboxbuild_depend>
<build_depend>controller_managerbuild_depend>
<build_depend>hardware_interfacebuild_depend>
<build_depend>joint_limits_interfacebuild_depend>
<build_depend>roscppbuild_depend>
<build_depend>sensor_msgsbuild_depend>
<build_depend>std_msgsbuild_depend>
<build_depend>trajectory_msgsbuild_depend>
<build_depend>transmission_interfacebuild_depend>
<run_depend>actionlibrun_depend>
<run_depend>urdfrun_depend>
<run_depend>cmake_modulesrun_depend>
<run_depend>control_msgsrun_depend>
<run_depend>control_toolboxrun_depend>
<run_depend>controller_managerrun_depend>
<run_depend>hardware_interfacerun_depend>
<run_depend>joint_limits_interfacerun_depend>
<run_depend>roscpprun_depend>
<run_depend>sensor_msgsrun_depend>
<run_depend>std_msgsrun_depend>
<run_depend>trajectory_msgsrun_depend>
<run_depend>transmission_interfacerun_depend>
<export>
export>
package>