ROS_Control框架介绍
- Controller：一个机器人包含多个controller，类似ros_controllers中现提供的、effort_controllers、joint_state_controller、position_controllers、velocity_controllers、joint_trajectory_controllers，不同的controller可以完成对不同模块的控制。例如完成对关节力的控制、速度控制等等。请求下层的硬件资源，并提供PID控制器，读取底层硬件资源的状态，发送控制指令。
Controller Manager：顾名思义，用来管理和控制各个controller的管理器，其主要提供统一的接口来管理不同的manager。
Hardware Rescource：为上下两层提供硬件资源的接口，内部有一个hardware resource manager来完成硬件资源管理。
RobotHW：硬件抽象层和硬件直接打交道，通过write和read方法来完成硬件的操作，这一层也包含关节限位、力矩转换、状态转换等功能。
Real Robot：实际的机器人上也需要有自己的嵌入式控制器，接收到命令后需要反映到执行器上，比如接收到位置1的命令后，那就需要让执行器快速、稳定的到达位置。
contorller_manager
此处我们看一下controller_manager.cpp的源码，可以看出通过一个控制机器加载器工具来进行初始化之后，最后再通过订阅六个service，完成了controller_manager的初始化，六个service分别是list_controllers、list_controller_types、load_controller、unload_controller、switch_controller、reload_controller_libraries，从其名字的字面意思也不难理解这六个服务的作用是什么。
加载控制器时，controller_manager将使用控制器名称作为所有控制器特定参数的根，最重要的是，标识要加载哪个插件的类型。控制器管理器提供与控制器交互的基础框架。根据你是从启动文件，命令行还是ROS节点运行控制器，控制器管理器提供不同的工具来运行控制器。
ControllerManager::ControllerManager(hardware_interface::RobotHW *robot_hw, const ros::NodeHandle& nh) :
robot_hw_(robot_hw),
root_nh_(nh),
cm_node_(nh, "controller_manager"),
start_request_(0),
stop_request_(0),
please_switch_(false),
current_controllers_list_(0),
used_by_realtime_(-1)
{
// create controller loader
controller_loaders_.push_back(
ControllerLoaderInterfaceSharedPtr(new ControllerLoader<controller_interface::ControllerBase>("controller_interface",
"controller_interface::ControllerBase") ) );
// Advertise services (this should be the last thing we do in init)
srv_list_controllers_ = cm_node_.advertiseService("list_controllers", &ControllerManager::listControllersSrv, this);
srv_list_controller_types_ = cm_node_.advertiseService("list_controller_types", &ControllerManager::listControllerTypesSrv, this);
srv_load_controller_ = cm_node_.advertiseService("load_controller", &ControllerManager::loadControllerSrv, this);
srv_unload_controller_ = cm_node_.advertiseService("unload_controller", &ControllerManager::unloadControllerSrv, this);
srv_switch_controller_ = cm_node_.advertiseService("switch_controller", &ControllerManager::switchControllerSrv, this);
srv_reload_libraries_ = cm_node_.advertiseService("reload_controller_libraries", &ControllerManager::reloadControllerLibrariesSrv, this);
}
可以使用controller_manager脚本从命令行与controller_manager进行交互，command包括:
load: 加载一个控制器（创建和初始化，但是不启动）
unload: 卸载一个控制器（销毁）
start: 启动一个控制器
stop: 停止一个控制器
spawn: 加载并启动一个控制器
kill: 停止并卸载一个控制器
rosrun controller_manager controller_manager <command> <controller_name>
要获取控制器的状态，command包括：
list: 按照执行顺序列出所有控制器，并给出每个控制器的状态
list-types: 列出控制器管理器知道的所有控制器类型。如果控制器不在此列表中，将无法获取。
reload-libraries: 重新加载所有可用作插件的控制器库。当您开发控制器并且想要测试新的控制器代码时，这很方便，而无需每次都重新启动机器人。而且不会重新启动之前运行的控制器
reload-libraries --restore: 重新加载所有可用作插件的控制器库，并将所有控制器恢复到其原始状态
rosrun controller_manager controller_manager <command>
还有其他命令行脚本工具来帮住我们完成不同的任务，具体的可以参考官网给出的说明，就不在此处进行罗列了。
我们还可以通过launch文件来启动我们的控制器，上面的一种是加载并启动，第二种是仅加载不启动。
<launch>
<node pkg="controller_manager"
type="spawner"
args="controller_name1 controller_name2" />
</launch>
<launch>
<node pkg="controller_manager"
type="spawner"
args="--stopped controller_name1 controller_name2" />
</launch>
还可以通过图形界面的方式对控制器进行操作，以图形方式加载，卸载，启动和停止控制器，以及显示加载的有关控制器的信息。
rosrun rqt_controller_manager rqt_controller_manager
controller_interface
#ifndef CONTROLLER_INTERFACE_CONTROLLER_H
#define CONTROLLER_INTERFACE_CONTROLLER_H
#include <controller_interface/controller_base.h>
#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/hardware_interface.h>
#include <ros/ros.h>
namespace controller_interface
{
/*
*具有特定硬件接口的控制器
* 模板类，该控制器使用的硬件接口类型。这强制了控制器与其要控制的硬件之间的语义兼容性。
*/
template <class T>
class Controller: public virtual ControllerBase
{
public:
Controller() {state_ = CONSTRUCTED;}
virtual ~Controller<T>(){}
/*
* 调用init函数来从非实时线程初始化控制器，其中指针指向硬件接口本身，而不是指向RobotHW的指针
* 参数 hw 此控制器使用的特定硬件接口
* 参数controller_nh命名空间中的NodeHandle，控制器应从中读取其配置，以及应在何处设置其ROS接口
* 初始化成功返回ture并且做好启动准备工作
*/
virtual bool init(T* /*hw*/, ros::NodeHandle& /*controller_nh*/) {return true;};
/*
* 参数controller_nh控制器命名空间中的NodeHandle。这是特定于控制器的配置所在的位置。
*/
virtual bool init(T* /*hw*/, ros::NodeHandle& /*root_nh*/, ros::NodeHandle& /*controller_nh*/) {return true;};
protected:
/*
* 从RobotHW指针初始化控制器
* 如果它可以从robot_hw中成功获取接口，则对控制器的硬件接口进行初始化
*/
virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& root_nh,
ros::NodeHandle& controller_nh,
ClaimedResources& claimed_resources)
{
// check if construction finished cleanly
if (state_ != CONSTRUCTED){
ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
return false;
}
// 获得硬件接口的指针
T* hw = robot_hw->get<T>();
if (!hw)
{
ROS_ERROR("This controller requires a hardware interface of type '%s'."
" Make sure this is registered in the hardware_interface::RobotHW class.",
getHardwareInterfaceType().c_str());
return false;
}
// 返回此控制器声明的资源
hw->clearClaims();
if (!init(hw, controller_nh) || !init(hw, root_nh, controller_nh))
{
ROS_ERROR("Failed to initialize the controller");
return false;
}
hardware_interface::InterfaceResources iface_res(getHardwareInterfaceType(), hw->getClaims());
claimed_resources.assign(1, iface_res);
hw->clearClaims();
// success
state_ = INITIALIZED;
return true;
}
/// 获取此控制器的硬件接口类型的名称
std::string getHardwareInterfaceType() const
{
return hardware_interface::internal::demangledTypeName<T>();
}
private:
Controller<T>(const Controller<T> &c);
Controller<T>& operator =(const Controller<T> &c);
};
}
#endif
从上面的源码中不难看出，contorller通过controlle_interface来完成hardware_interface的初始化。
hardware_interface
最重要的交互接口，与机器人硬件完成连接，我们通过类图来理清接口之间关系。
这里需要补充说明的是HardwareResourceManager通过getHandle获取到不同的handle对应到到图中右侧的不同的HardwareInterface。这个在类图上并没有体现出来，流程图如下：
