ROS中目前已经支持的controllers主要包括:
1)diff_drive_controller
2)effort_controllers
3)force_torque_sensor_controller
4)forward_command_controller
5)gripper_action_controller
6)imu_sensor_controller
7)joint_state_controller
8)joint_trajectory_controller
9)position_controllers
10)rqt_joint_trajectory_controller
11)velocity_controllers
在ROS中这些controller通常被实现成插件,并通过controller manager来加载调用。
1 主要数据结构
1.1 controller_interface::ControllerBase
源码注释如下:
class ControllerBase
{
public:
ControllerBase(): state_(CONSTRUCTED){}
virtual ~ControllerBase(){}
//This is called from within the realtime thread just before the first call to update
virtual void starting(const ros::Time& /*time*/) {};
//This is called periodically by the realtime thread when the controller is running.
virtual void update(const ros::Time& time, const ros::Duration& period) = 0;
//This is called from within the realtime thread just after the last update call before the controller is stopped.
virtual void stopping(const ros::Time& /*time*/) {};
//Check if the controller is running
bool isRunning()
{
return (state_ == RUNNING);
}
void updateRequest(const ros::Time& time, const ros::Duration& period)
{
if (state_ == RUNNING)
update(time, period);
}
bool startRequest(const ros::Time& time)
{
// start succeeds even if the controller was already started
if (state_ == RUNNING || state_ == INITIALIZED){
starting(time);
state_ = RUNNING;
return true;
}
else
return false;
}
bool stopRequest(const ros::Time& time)
{
// stop succeeds even if the controller was already stopped
if (state_ == RUNNING || state_ == INITIALIZED){
stopping(time);
state_ = INITIALIZED;
return true;
}
else
return false;
}
typedef std::vector ClaimedResources;
//Request that the controller be initialized.
//Parameters
// robot_hw The robot hardware abstraction.
// root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).
// controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific configuration resides.
//[out] claimed_resources The resources claimed by this controller. They can belong to multiple hardware interfaces.
//Returns
//True if initialization was successful and the controller is ready to be started
virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& root_nh,
ros::NodeHandle& controller_nh,
ClaimedResources& claimed_resources) = 0;
enum {CONSTRUCTED, INITIALIZED, RUNNING} state_;
private:
ControllerBase(const ControllerBase &c);
ControllerBase& operator =(const ControllerBase &c);
};
1.2 controller_interface::Controller< T >
源码注释如下,
template
class Controller: public ControllerBase
{
public:
Controller() {state_ = CONSTRUCTED;}
virtual ~Controller(){}
//The init function is called to initialize the controller from a non-realtime thread with a pointer to the hardware interface, itself, instead of a pointer to a RobotHW.
//Parameters
//hw The specific hardware interface used by this controller.
/controller_nh A NodeHandle in the namespace from which the controller should read its configuration, and where it should set up its ROS interface.
//Returns
//True if initialization was successful and the controller is ready to be started.
virtual bool init(T* /*hw*/, ros::NodeHandle& /*controller_nh*/) {return true;};
//The init function is called to initialize the controller from a non-realtime thread with a pointer to the hardware interface, itself, instead of a pointer to a RobotHW.
//Parameters
//hw The specific hardware interface used by this controller.
//root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).
//controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific configuration resides.
//Returns
//True if initialization was successful and the controller is ready to be started.
virtual bool init(T* /*hw*/, ros::NodeHandle& /*root_nh*/, ros::NodeHandle& /*controller_nh*/) {return true;};
protected:
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;
}
// get a pointer to the hardware interface
T* hw = robot_hw->get();
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;
}
// return which resources are claimed by this controller
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;
}
//Get the name of this controller's hardware interface type
std::string getHardwareInterfaceType() const
{
return hardware_interface::internal::demangledTypeName();
}
private:
Controller(const Controller &c);
Controller& operator =(const Controller &c);
};
1.3 controller_interface::MultiInterfaceController
template
class MultiInterfaceController: public ControllerBase
{
public:
//allow_optional_interfaces If set to true, initRequest will not fail if one or more of the requested interfaces is not present.
//If set to false (the default), all requested interfaces are required.
MultiInterfaceController(bool allow_optional_interfaces = false)
: allow_optional_interfaces_(allow_optional_interfaces)
{state_ = CONSTRUCTED;}
virtual ~MultiInterfaceController() {}
//Custom controller initialization logic.
//In this method resources from different interfaces are claimed, and other non real-time initialization is performed, such as setup of ROS interfaces and resource pre-allocation.
//Parameters
//robot_hw Robot hardware abstraction containing a subset of the entire robot.
//If MultiInterfaceController was called with allow_optional_interfaces set to false (the default), this parameter contains all the interfaces requested by the controller.
//If allow_optional_interfaces was set to false, this parameter may contain none, some or all interfaces requested by the controller, depending on whether the robot exposes them.
//controller_nh A NodeHandle in the namespace from which the controller should read its configuration, and where it should set up its ROS interface.
//Returns
//True if initialization was successful and the controller is ready to be started.
virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
ros::NodeHandle& /*controller_nh*/)
{return true;}
virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
ros::NodeHandle& /*root_nh*/,
ros::NodeHandle& /*controller_nh*/)
{return true;}
protected:
//Initialize the controller from a RobotHW pointer.
//This calls init with a RobotHW that is a subset of the input robot_hw parameter,
//containing only the requested hardware interfaces (all or some, depending on the value of allow_optional_interfaces passed to the constructor).
//Parameters
//robot_hw The robot hardware abstraction.
//root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).
//controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific configuration resides.
//[out] claimed_resources The resources claimed by this controller. They can belong to multiple hardware interfaces.
//Returns
//True if initialization was successful and the controller is ready to be started.
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;
}
// check for required hardware interfaces
if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) {return false;}
// populate robot hardware abstraction containing only controller hardware interfaces (subset of robot)
hardware_interface::RobotHW* robot_hw_ctrl_p = &robot_hw_ctrl_;
extractInterfaceResources(robot_hw, robot_hw_ctrl_p);
// custom controller initialization
clearClaims(robot_hw_ctrl_p); // claims will be populated on controller init
if (!init(robot_hw_ctrl_p, controller_nh) || !init(robot_hw_ctrl_p, root_nh, controller_nh))
{
ROS_ERROR("Failed to initialize the controller");
return false;
}
// populate claimed resources
claimed_resources.clear();
populateClaimedResources(robot_hw_ctrl_p, claimed_resources);
//Clear claims from all hardware interfaces requested by this controller.
//Parameters
//robot_hw Robot hardware abstraction containing the interfaces whose claims will be cleared.
clearClaims(robot_hw_ctrl_p);
// NOTE: Above, claims are cleared since we only want to know what they are and report them back
// as an output parameter. Actual resource claiming by the controller is done when the controller
// is start()ed
// initialization successful
state_ = INITIALIZED;
return true;
}
/*\}*/
//Check if robot hardware abstraction contains all required interfaces.
//Parameters
//robot_hw Robot hardware abstraction.
//Returns
//true if all required hardware interfaces are exposed by robot_hw, false otherwise.
static bool hasRequiredInterfaces(hardware_interface::RobotHW* robot_hw)
{
using internal::hasInterface;
return hasInterface(robot_hw) &&
hasInterface(robot_hw) &&
hasInterface(robot_hw) &&
hasInterface(robot_hw);
}
static void clearClaims(hardware_interface::RobotHW* robot_hw)
{
using internal::clearClaims;
clearClaims(robot_hw);
clearClaims(robot_hw);
clearClaims(robot_hw);
clearClaims(robot_hw);
}
//Extract all hardware interfaces requested by this controller from robot_hw_in, and add them also to robot_hw_out.
//Parameters
//[in] robot_hw_in Robot hardware abstraction containing the interfaces requested by this controller, and potentially others.
//[out] robot_hw_out Robot hardware abstraction containing only the interfaces requested by this controller.
static void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in,
hardware_interface::RobotHW* robot_hw_out)
{
using internal::extractInterfaceResources;
extractInterfaceResources(robot_hw_in, robot_hw_out);
extractInterfaceResources(robot_hw_in, robot_hw_out);
extractInterfaceResources(robot_hw_in, robot_hw_out);
extractInterfaceResources(robot_hw_in, robot_hw_out);
}
//Parameters
//[in] robot_hw_in Robot hardware abstraction containing the interfaces requested by this controller, and potentially others.
//[out] claimed_resources The resources claimed by this controller. They can belong to multiple hardware interfaces.
static void populateClaimedResources(hardware_interface::RobotHW* robot_hw,
ClaimedResources& claimed_resources)
{
using internal::populateClaimedResources;
populateClaimedResources(robot_hw, claimed_resources);
populateClaimedResources(robot_hw, claimed_resources);
populateClaimedResources(robot_hw, claimed_resources);
populateClaimedResources(robot_hw, claimed_resources);
}
//Robot hardware abstraction containing only the subset of interfaces requested by the controller.
hardware_interface::RobotHW robot_hw_ctrl_;
//Flag to indicate if hardware interfaces are considered optional (i.e. non-required).
bool allow_optional_interfaces_;
private:
MultiInterfaceController(const MultiInterfaceController& c);
MultiInterfaceController& operator =(const MultiInterfaceController& c);
};
2 controller示例
如下controller在hardware interface(EffortJointInterface)上track position command。
#include
#include
#include
namespace controller_ns{
class PositionController : public controller_interface::Controller
{
public:
bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
{
// get joint name from the parameter server
std::string my_joint;
if (!n.getParam("joint", my_joint)){
ROS_ERROR("Could not find joint name");
return false;
}
// get the joint object to use in the realtime loop
joint_ = hw->getHandle(my_joint); // throws on failure
return true;
}
void update(const ros::Time& time, const ros::Duration& period)
{
double error = setpoint_ - joint_.getPosition();
joint_.setCommand(error*gain_);
}
void starting(const ros::Time& time) { }
void stopping(const ros::Time& time) { }
private:
hardware_interface::JointHandle joint_; //resource handle
static const double gain_ = 1.25;
static const double setpoint_ = 3.00; //desired position
};
PLUGINLIB_DECLARE_CLASS(package_name, PositionController, controller_ns::PositionController, controller_interface::ControllerBase);
}//namespace