来源
ros_control 脱胎于pr2 的 硬件封装层部分 pr2_mechanism,经过pal-robotics 和 hidof 两个公司的工程师进行了改写,变得适用于所有机器人的硬件封装库,负责统一管理硬件驱动与传感器底层细节,处理异常,分配资源,向上提供统一接口。作为一个end2end的ROS 机器人系统集成解决方案。
优势
ros_control 是一个类库!! 并不是直接的工具库,需要自己写程序继承特定基类来做到接口的统一。
Ros_control 使用的最直接好处是可以复用 ros-controllers 里面的一些控制算法,以及与支持ros_control的仿真器 gazebo 的良好支持。
缺点
ros_control 框架最大的问题,很多模块严重冗余,很多部分设计突出一个没必要,架构过于复杂,很多模块象征意义大于实际意义,更多是提供一个系统集成的思路或者雏形,具体完善到工业级还得好长的路要走,学习曲线陡峭没文档。
基本架构
基本架构是这样的:
机器人的硬件通讯库(串口,can, ethercat, 等等)
ros_control 的robot_hw部分
controller_manager 部分 与具体的 controller
controller 部分就会提供给ROS系统中其他上层组件标准控制与感知接口,比如 topic : cmd_vel,joint/command action:trajectory_server 等等。
简单实例
Controller_manager 与很多controller都已经很完善了,需要写的就是robothw部分
一个最简单的ros_control例子:
在diffbot.h 中定义了一个双轮差速小车的模型。
Diffbot 类中定义了小车的joint 信息(位置,速度,扭矩? 力? ,pos, vel,effort),来描述关节的状态,
又定义了cmd来存储对关节的控制量。
设置了不同的hardware_interface对相应数据进行注册,
然后将不同的控制接口注册到robothw上.
实现了 两个虚方法 read 和write ,因为是个demo,所以处理数据非常简单。
源码如下
template <unsigned int NUM_JOINTS = 2>
class Diffbot : public hardware_interface::RobotHW
{
public:
Diffbot()//构造函数
: running_(true)
, start_srv_(nh_.advertiseService("start", &Diffbot::start_callback, this))
, stop_srv_(nh_.advertiseService("stop", &Diffbot::stop_callback, this))
{
// Intialize raw data //初始化原始数据 在私有部分有声明该四个变量
std::fill_n(pos_, NUM_JOINTS, 0.0);
std::fill_n(vel_, NUM_JOINTS, 0.0);
std::fill_n(eff_, NUM_JOINTS, 0.0);
std::fill_n(cmd_, NUM_JOINTS, 0.0);
// Connect and register the joint state and velocity interface
for (unsigned int i = 0; i < NUM_JOINTS; ++i)
{
std::ostringstream os;
os << "wheel_" << i << "_joint";
// 设置不同的hardware_interface对应数据 并 进行注册,
hardware_interface::JointStateHandle state_handle(os.str(), &pos_[i], &vel_[i], &eff_[i]);//设置jointstate 接口数据
jnt_state_interface_.registerHandle(state_handle);
hardware_interface::JointHandle vel_handle(jnt_state_interface_.getHandle(os.str()), &cmd_[i]);//设置joint vel handle 接口数据
jnt_vel_interface_.registerHandle(vel_handle);
}
//进行注册
registerInterface(&jnt_state_interface_);
registerInterface(&jnt_vel_interface_);
}
ros::Time getTime() const {return ros::Time::now();}
ros::Duration getPeriod() const {return ros::Duration(0.01);}
void read()
{
std::ostringstream os;
for (unsigned int i = 0; i < NUM_JOINTS - 1; ++i)
{
os << cmd_[i] << ", ";
}
os << cmd_[NUM_JOINTS - 1];
ROS_INFO_STREAM("Commands for joints: " << os.str());
}
void write()
{
if (running_)
{
for (unsigned int i = 0; i < NUM_JOINTS; ++i)
{
// Note that pos_[i] will be NaN for one more cycle after we start(),
// but that is consistent with the knowledge we have about the state
// of the robot.
pos_[i] += vel_[i]*getPeriod().toSec(); // update position
vel_[i] = cmd_[i]; // might add smoothing here later
}
}
else
{
std::fill_n(pos_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
std::fill_n(vel_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
}
}
bool start_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
{
running_ = true;
return true;
}
bool stop_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
{
running_ = false;
return true;
}
private:
hardware_interface::JointStateInterface jnt_state_interface_;//声明joint state 接口 类内部实例
hardware_interface::VelocityJointInterface jnt_vel_interface_;//声明joint vel接口 类内部实例
double cmd_[NUM_JOINTS];//存储对关节的控制量
double pos_[NUM_JOINTS];//joint位置信息
double vel_[NUM_JOINTS];//joint速度信息
double eff_[NUM_JOINTS];//joint力信息
bool running_;
ros::NodeHandle nh_;
ros::ServiceServer start_srv_;
ros::ServiceServer stop_srv_;
};
在diffbot.cpp 中给出了ros_control的运行流程
将Diffbot (继承于hardware_interface::RobotHW)类传给ControlerManager,ControlerManager会查看相对应hardware_interface,使用的时候通过service告诉controller_manager load对应类型及其相应的参数(launch文件种),启动controller,即可
源码如下
// ROS
#include <ros/ros.h>
// ros_control
#include <controller_manager/controller_manager.h>
#include "diffbot.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "diffbot");
ros::NodeHandle nh;
Diffbot<> robot;
ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
controller_manager::ControllerManager cm(&robot, nh);
ros::Rate rate(1.0 / robot.getPeriod().toSec());//制定数据更新周期
ros::AsyncSpinner spinner(1);
spinner.start();
while(ros::ok())
{
robot.read();//读数据
cm.update(robot.getTime(), robot.getPeriod());//更新数据
robot.write();//将更新过的数据写数据
rate.sleep();
}
spinner.stop();
return 0;
}
ros_control核心概念
hardware_interface:作为ros_control组件最重要的一部分,做了以下抽象:
将能动的部分(类似于机器人关节)抽象为joint,joint类型有旋转,平动,固定等等。
执行器模型抽象为actuator,与joint不同的是actuator的属性值需要一定变换才能对应到joint可以理解为电机减速,或者机构传动。
根据不同的控制方式或者不同的传感器暴露出相应的数据接口,一般移动机器人底盘是速度闭环,而机械臂上是位置闭环,根据这些控制方法的不同分出了不同的控制接口,暴露给上层的controller。
作为机器人的硬件资源与上层的直接接口,可以被运行时产生与删除,结合机器人本身的通讯组件这样实际上实现了一种机器人硬件资源的低层次管理。
而hardware_interface 具体实现方式是存储对应状态变量的指针,用字符串表示不同的joint与 Actuator 资源。robothw 基类是 硬件通讯库与hardware_interface交互的部分。硬件通讯库具体的读写过程都在read 与 write 两个虚方法中实现,更新的数据放在robothw类的成员变量中,这些存储着joint 与actuator 状态与命令的空间被hardware_interface索引,传给controller_manager ,通过controller_manager的接口将数据接给controller。所以ros_control内部不存在进程间通讯。