ROS_Control框架介紹
- Controller：一個(gè)機(jī)器人包含多個(gè)controller堤结，類似ros_controllers中現(xiàn)提供的、effort_controllers肉拓、joint_state_controller后频、position_controllers、velocity_controllers暖途、joint_trajectory_controllers卑惜，不同的controller可以完成對不同模塊的控制。例如完成對關(guān)節(jié)力的控制驻售、速度控制等等露久。請求下層的硬件資源，并提供PID控制器欺栗，讀取底層硬件資源的狀態(tài)毫痕，發(fā)送控制指令征峦。
Controller Manager：顧名思義，用來管理和控制各個(gè)controller的管理器消请，其主要提供統(tǒng)一的接口來管理不同的manager栏笆。
Hardware Rescource：為上下兩層提供硬件資源的接口，內(nèi)部有一個(gè)hardware resource manager來完成硬件資源管理臊泰。
RobotHW：硬件抽象層和硬件直接打交道蛉加，通過write和read方法來完成硬件的操作，這一層也包含關(guān)節(jié)限位缸逃、力矩轉(zhuǎn)換针饥、狀態(tài)轉(zhuǎn)換等功能。
Real Robot：實(shí)際的機(jī)器人上也需要有自己的嵌入式控制器需频，接收到命令后需要反映到執(zhí)行器上丁眼，比如接收到位置1的命令后，那就需要讓執(zhí)行器快速昭殉、穩(wěn)定的到達(dá)位置户盯。
contorller_manager
此處我們看一下controller_manager.cpp的源碼，可以看出通過一個(gè)控制機(jī)器加載器工具來進(jìn)行初始化之后饲化，最后再通過訂閱六個(gè)service莽鸭，完成了controller_manager的初始化，六個(gè)service分別是list_controllers吃靠、list_controller_types硫眨、load_controller、unload_controller巢块、switch_controller礁阁、reload_controller_libraries，從其名字的字面意思也不難理解這六個(gè)服務(wù)的作用是什么族奢。
加載控制器時(shí)姥闭，controller_manager將使用控制器名稱作為所有控制器特定參數(shù)的根，最重要的是越走，標(biāo)識(shí)要加載哪個(gè)插件的類型棚品。控制器管理器提供與控制器交互的基礎(chǔ)框架廊敌。根據(jù)你是從啟動(dòng)文件铜跑，命令行還是ROS節(jié)點(diǎn)運(yùn)行控制器，控制器管理器提供不同的工具來運(yùn)行控制器骡澈。
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進(jìn)行交互锅纺，command包括:
load: 加載一個(gè)控制器（創(chuàng)建和初始化，但是不啟動(dòng)）
unload: 卸載一個(gè)控制器（銷毀）
start: 啟動(dòng)一個(gè)控制器
stop: 停止一個(gè)控制器
spawn: 加載并啟動(dòng)一個(gè)控制器
kill: 停止并卸載一個(gè)控制器
rosrun controller_manager controller_manager <command> <controller_name>
要獲取控制器的狀態(tài)肋殴，command包括：
list: 按照執(zhí)行順序列出所有控制器囤锉，并給出每個(gè)控制器的狀態(tài)
list-types: 列出控制器管理器知道的所有控制器類型坦弟。如果控制器不在此列表中，將無法獲取官地。
reload-libraries: 重新加載所有可用作插件的控制器庫减拭。當(dāng)您開發(fā)控制器并且想要測試新的控制器代碼時(shí)，這很方便区丑，而無需每次都重新啟動(dòng)機(jī)器人。而且不會(huì)重新啟動(dòng)之前運(yùn)行的控制器
reload-libraries --restore: 重新加載所有可用作插件的控制器庫修陡，并將所有控制器恢復(fù)到其原始狀態(tài)
rosrun controller_manager controller_manager <command>
還有其他命令行腳本工具來幫住我們完成不同的任務(wù)沧侥，具體的可以參考官網(wǎng)給出的說明，就不在此處進(jìn)行羅列了魄鸦。
我們還可以通過launch文件來啟動(dòng)我們的控制器宴杀，上面的一種是加載并啟動(dòng)，第二種是僅加載不啟動(dòng)拾因。
<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>
還可以通過圖形界面的方式對控制器進(jìn)行操作旺罢，以圖形方式加載，卸載绢记，啟動(dòng)和停止控制器扁达，以及顯示加載的有關(guān)控制器的信息。
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
{
/*
*具有特定硬件接口的控制器
* 模板類蠢熄，該控制器使用的硬件接口類型跪解。這強(qiáng)制了控制器與其要控制的硬件之間的語義兼容性。
*/
template <class T>
class Controller: public virtual ControllerBase
{
public:
Controller() {state_ = CONSTRUCTED;}
virtual ~Controller<T>(){}
/*
* 調(diào)用init函數(shù)來從非實(shí)時(shí)線程初始化控制器签孔，其中指針指向硬件接口本身叉讥，而不是指向RobotHW的指針
* 參數(shù) hw 此控制器使用的特定硬件接口
* 參數(shù)controller_nh命名空間中的NodeHandle，控制器應(yīng)從中讀取其配置饥追，以及應(yīng)在何處設(shè)置其ROS接口
* 初始化成功返回ture并且做好啟動(dòng)準(zhǔn)備工作
*/
virtual bool init(T* /*hw*/, ros::NodeHandle& /*controller_nh*/) {return true;};
/*
* 參數(shù)controller_nh控制器命名空間中的NodeHandle图仓。這是特定于控制器的配置所在的位置。
*/
virtual bool init(T* /*hw*/, ros::NodeHandle& /*root_nh*/, ros::NodeHandle& /*controller_nh*/) {return true;};
protected:
/*
* 從RobotHW指針初始化控制器
* 如果它可以從robot_hw中成功獲取接口但绕，則對控制器的硬件接口進(jìn)行初始化
*/
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
最重要的交互接口捏顺，與機(jī)器人硬件完成連接帚豪，我們通過類圖來理清接口之間關(guān)系。
這里需要補(bǔ)充說明的是HardwareResourceManager通過getHandle獲取到不同的handle對應(yīng)到到圖中右側(cè)的不同的HardwareInterface草丧。這個(gè)在類圖上并沒有體現(xiàn)出來狸臣，流程圖如下：
