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_hwThe robot hardware abstraction.    //root_nhA NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).    //controller_nhA NodeHandle in the namespace of the controller. This is where the controller-specific configuration resides.    //[out]claimed_resourcesThe 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     //hwThe specific hardware interface used by this controller.     /controller_nhA 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      //hwThe specific hardware interface used by this controller.      //root_nhA NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).      //controller_nhA 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_interfacesIf 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_hwRobot 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_nhA 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_hwThe robot hardware abstraction.    //root_nhA NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).    //controller_nhA NodeHandle in the namespace of the controller. This is where the controller-specific configuration resides.        //[out]claimed_resourcesThe 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_hwRobot 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_hwRobot 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_inRobot hardware abstraction containing the interfaces requested by this controller, and potentially others.     //[out]robot_hw_outRobot 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_inRobot hardware abstraction containing the interfaces requested by this controller, and potentially others.    //[out]claimed_resourcesThe 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