ROS controllers简介

   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<hardware_interface::InterfaceResources> 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 T>
   class Controller: public ControllerBase
    {
    public:
      Controller()  {state_ = CONSTRUCTED;}
      virtual ~Controller<T>(){}
    
      //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<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;
       }
   
       // 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<T>();
     }
   
   private:
     Controller<T>(const Controller<T> &c);
     Controller<T>& operator =(const Controller<T> &c);
  
   };

1.3 controller_interface::MultiInterfaceController

template <class T1, class T2 = void, class T3 = void, class T4 = void>
   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<T1>(robot_hw) &&
             hasInterface<T2>(robot_hw) &&
              hasInterface<T3>(robot_hw) &&
              hasInterface<T4>(robot_hw);
     }
   
     static void clearClaims(hardware_interface::RobotHW* robot_hw)
     {
       using internal::clearClaims;
       clearClaims<T1>(robot_hw);
       clearClaims<T2>(robot_hw);
       clearClaims<T3>(robot_hw);
       clearClaims<T4>(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<T1>(robot_hw_in, robot_hw_out);
       extractInterfaceResources<T2>(robot_hw_in, robot_hw_out);
       extractInterfaceResources<T3>(robot_hw_in, robot_hw_out);
      extractInterfaceResources<T4>(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<T1>(robot_hw, claimed_resources);
       populateClaimedResources<T2>(robot_hw, claimed_resources);
       populateClaimedResources<T3>(robot_hw, claimed_resources);
       populateClaimedResources<T4>(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 <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>

namespace controller_ns{

class PositionController : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
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