使用ros_control ros_controllers 的牛刀真实驱动舵机手臂的源码

现场

rqt_graph

在一个陌生的框架下写代码，免不了有很多疑问与槽点。

不了解框架结构，千头万续，无从下手，说不清，理还乱。
资料少没有文档，要读懂程序猿的心，就只得静下心来穿针引线般的读代码了。
让我们从这里开始吧————这到底是个什么鬼？慢慢来，哈哈

原标题
ros_control是个什么东西？
http://www.360doc.com/content/16/0509/17/33166754_557610167.shtml


ROS control-----hardware_interface简介
http://blog.csdn.net/x_r_su/article/details/53284477

RosControl学习
http://blog.csdn.net/sunanger/article/details/53033630

官方
ros_control
http://wiki.ros.org/ros_control

还是这张图看上100遍

读了这几篇，从大框框上认识一下，需要注意的点做个总结。（部分描述copy原文）
ros_control定义了机器人硬件接口和机器人控制器的抽象接口，控制器管理类controller_manager类，
其网址为： https://github.com/ros-controls/ros_control
控制器(ros_controllers)的实现类是在ros_controllers包内实现的，
其网址为： https://github.com/shadow-robot/ros_controllers

ros_control  ros_controllers密切联系点
ros_controllers包中每个XXXController继承了ros_control包内的Controller

深入源码看看

ros_control包下controller_interface空间中Controller类

class Controller: public ControllerBase

抽象类ControllerBase中虚函数starting，stoping，update
ros_controllers包下joint_trajectory_controller空间中JointTrajectoryController类

class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>

实现了

bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);  
void starting(const ros::Time& time);
void stopping(const ros::Time& /*time*/);

void update(const ros::Time& time, const ros::Duration& period);

注：JointTrajectoryController本质上是actionserver，驱动机械臂时，它和moveit的client进行交互。




gazebo仿真时
自定义插件类必须继承自gazebo_ros_control::RobotHWSim实现了模拟ros_control包中的hardware_interface::RobotHW
插件类必须实现initSim readSim writeSim

驱动真实硬件时用到ros_control类库 也要实现gezabo插件类类似的功能
写一个类继承 hardware_interface::RobotHW，用来实现各种硬件接口的注册
RobotHW 类派生自InterfaceManger
InterfaceManger类为接口管理类，提供有registerInterface函数，用于注册机器人硬件的接口类（**Interface），并可以用T* Get（）函数获取接口类的对象

hardware_interface::JointStateInterface    js_interface_;
hardware_interface::PositionJointInterface pj_interface_;
//....
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&pj_interface_);

JointStateInterface继承了HardwareResourceManager

class JointStateInterface : public HardwareResourceManager<JointStateHandle> {}

又
HardwareResourceManger类继承了HardwareInterface和ResourceManger<ResourceHandle>，

class HardwareResourceManager : public HardwareInterface, public ResourceManager<ResourceHandle>

HardwareInterface
提供claim（）和clearClaim（）函数用于声索资源和清除声索资源；
ResourceManger<ResourceHandle>
提供registerHandle（ResourceHandle handle）向接口类注册资源handle
提供gethandle（name）获取handle对象。

层层继承下来所以JointStateInterface具有registerHandle（ResourceHandle handle）gethandle（name)功能。

unsigned int n_dof_;
std::vector<std::string> joint_names_;
std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
std::vector<double> joint_effort_;
std::vector<double> joint_position_command_;


joint_names_.push_back("_joint0");
joint_names_.push_back("_joint1");
joint_names_.push_back("_joint2");

n_dof_ = joint_names_.size();

joint_position_.resize(n_dof_);
joint_velocity_.resize(n_dof_);
joint_effort_.resize(n_dof_);
joint_position_command_.resize(n_dof_);


for (std::size_t i = 0; i < n_dof_; ++i)
{
// Create joint state interface for all joints
//JointstateHandle为关节状态的资源类，其内部存储有：pos，vel，effort的获取接口。

js_interface.registerHandle(hardware_interface::JointStateHandle(
        joint_names_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]));




// Create position joint interface
/*
类似于JointStateInterface和JointstateHandle的一组接口资源类，还有JointHandle和JointCommandInterface，其中JointHandle不但包含了Jointstatehandle的所有内容，还包括了setcommand和getcommand两个命令接口，从这个JointCommandInterface类还派生出了PositionJointInterface，VelocityJointInterface，EffortJointInterface等类，其本质是一回事，就是实现一个控制量的读取和写入接口，同时获取joint的状态。
*/
    pj_interface.registerHandle(hardware_interface::JointHandle(
        js_interface.getHandle(joint_names_[i]),&joint_position_command_[i]));
}

官方简例代码
https://github.com/ros-controls/ros_controllers/blob/indigo-devel/diff_drive_controller/test/diffbot.h

https://github.com/ros-controls/ros_controllers/blob/indigo-devel/diff_drive_controller/test/diffbot.cpp


不难看出要让controller manager和controller协作，就须遵循下面的套路。

main()
{
   MyRobot robot;//这个class MyRobot 继承 hardware_interface::RobotHW实现read() write()
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read(); //to access hardware driver， generally need do some wrap and customization in MyRobot class
     cm.update(robot.get_time(), robot.get_period());
     robot.write(); //to access hardware driver， generally need do some wrap and customization in MyRobot class
     sleep();
  }
}

实现read write 的实现并不一定在robot类里，看具体需求，只要在合适的位置循环调用就好了，如下文官方给的baxter实例的read和write，并且是使用定时器触发代替循环的，详细请自行慢慢观摩。
实现硬件协议可以是串口 can总线或其他，我这里是用串口，关于驱动源码请看下面裸跑实例内的链接。
驱动手臂只要能给硬件下发消息，能读回关节状态就好。
其实抛弃ros_contrl裸跑其实更直观/简单/更好理解一些。
裸跑实例源码。
http://blog.csdn.net/qq_38288618/article/details/78078514
两点
moveit 控制真实机器手臂时需要controller接收 moveit的路径消息，然后把消息write到硬件上。
moveit 需要read机器手臂的状态join_states


使用ros_control的好处
只需把yaml写好 JointTrajectoryController和moveit自动取得联系。
不用自己处理actionserver
框架功能强大

让我们开始创作代码吧
......怎么，线头太多不知从何处开始，好吧，似乎还是写不出来一个实例。
那让我们找个源码做手术吧，改改改。
官方提供了Baxter的代码实例
Rethink Baxter hardware as used at the University of Colorado Boulder: Baxter on Github
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/baxter_control/src/baxter_hardware_interface.cpp


https://github.com/davetcoleman/baxter_cpp/整个down下来
就拿它开刀！目的就是嫁接在自己的舵机手臂上能使。
仿真的部分略过不做处理，只拿自己要的。不需要的部分注释加删除，按需要添加改名。

主要修改双手臂为单手臂，关节名称，尝试调用PID的yaml信息等等,实现真实手臂运动规划的走位。
（抓手的controller没有做，由于抓手是一个电机控制两个抓手开合，应该需要写一个简单映射或者动力学的公式才行，这里翻篇）。


冗长代码开始之前先看看Baxter


代码开启

arm_interface.h

#ifndef BAXTER_CONTROL__ARM_INTERFACE_
#define BAXTER_CONTROL__ARM_INTERFACE_

// Boost
#include <boost/shared_ptr.hpp>

// ROS
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>

// ros_control
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
//#include <hardware_interface/joint_mode_interface.h>

// Baxter
//#include <baxter_core_msgs/JointCommand.h>
//#include <baxter_core_msgs/JointCommand.h>


namespace zzz_arm_2_control
{

enum BaxterControlMode { POSITION, VELOCITY, TORQUE };

class ArmInterface
{
protected:

  // Node Handles
  ros::NodeHandle nh_; // no namespace

  // Number of joints we are using
  unsigned int n_dof_;

  std::vector<std::string> joint_names_;
  std::vector<double> joint_position_;
  std::vector<double> joint_velocity_;
  std::vector<double> joint_effort_;
  std::vector<double> joint_position_command_;
  std::vector<double> joint_effort_command_;
  std::vector<double> joint_velocity_command_;

  // Track current hardware interface mode we are in
  int* joint_mode_;

  // Speed of hardware loop
  double loop_hz_;

  // Name of this arm
  std::string arm_name_;

public:

  /**
   * \brief Constructor/Descructor
   */
  ArmInterface(const std::string &arm_name, double loop_hz)
    : arm_name_(arm_name),
      loop_hz_(loop_hz)
  {};

  ~ArmInterface()
  {};

  /**
   * \brief Initialice hardware interface
   * \return false if an error occurred during initialization
   */
  virtual bool init(
    hardware_interface::JointStateInterface&    js_interface,
    hardware_interface::EffortJointInterface&   ej_interface,
    hardware_interface::VelocityJointInterface& vj_interface,
    hardware_interface::PositionJointInterface& pj_interface,
    int* joint_mode,
    sensor_msgs::JointStateConstPtr state_msg
  )
  { return true; };

  /**
   * \brief Copy the joint state message into our hardware interface datastructures
   */
  virtual void read( sensor_msgs::JointStateConstPtr &state_msg )
  {};

  /**
   * \brief Publish our hardware interface datastructures commands to Baxter hardware
   */
  virtual void write(ros::Duration elapsed_time)
  {};

  /**
   * \brief This is called when Baxter is disabled, so that we can update the desired positions
   */
  virtual void robotDisabledCallback()
  {};

};

typedef boost::shared_ptr<zzz_arm_2_control::ArmInterface> ArmInterfacePtr;
typedef boost::shared_ptr<const zzz_arm_2_control::ArmInterface> ArmInterfaceConstPtr;

} // namespace

#endif

arm_hardware_interface.h

#ifndef BAXTER_CONTROL__ARM_HARDWARE_INTERFACE_
#define BAXTER_CONTROL__ARM_HARDWARE_INTERFACE_

// ROS
#include <trajectory_msgs/JointTrajectory.h>

// Baxter
//#include <baxter_core_msgs/JointCommand.h>
//#include <baxter_core_msgs/DigitalIOState.h>

// Parent class
#include <zzz_arm_2_hardware/arm_interface.h>
#include <vector>
#include <string>
#include <control_toolbox/pid.h>
#include <joint_limits_interface/joint_limits_interface.h>

namespace zzz_arm_2_control
{

static const double STATE_EXPIRED_TIMEOUT = 2.0;

class ArmHardwareInterface : public ArmInterface
{
private:

  // Publishers
  ros::Publisher pub_joint_command_;
  ros::Publisher pub_trajectory_command_;
  ros::Publisher zzz_fake_joint_pub_ ;

  ros::Publisher joint_motor_pub_ ;
  // Subscriber
  ros::Subscriber cuff_squeezed_sub_; // this is used to update the controllers when manual mode is started

  // Messages to send
  //baxter_core_msgs::JointCommand output_msg_;
  trajectory_msgs::JointTrajectory trajectory_command_msg_;

  // Track button status
  bool cuff_squeezed_previous;

  // Convert a joint states message to our ids
  std::vector<int> joint_id_to_joint_states_id_;

  //zzzadd
  // Joint limits interface
  joint_limits_interface::PositionJointSoftLimitsInterface jnt_limits_interface_;

  // PID controllers
  std::vector<control_toolbox::Pid> pids_;

  template <class T>
  std::string containerToString(const T& cont, const std::string& prefix)
  {
    std::stringstream ss;
    ss << prefix;
    std::copy(cont.begin(), --cont.end(), std::ostream_iterator<typename T::value_type>(ss, prefix.c_str()));
    ss << *(--cont.end());
    return ss.str();
  }

public:

  /**
   * \brief Constructor/Descructor
   */
  ArmHardwareInterface(const std::string &arm_name, double loop_hz);
  ~ArmHardwareInterface();

  /**
   * \brief Initialice hardware interface
   * \return false if an error occurred during initialization
   */
  bool init(
    hardware_interface::JointStateInterface&    js_interface,
    hardware_interface::EffortJointInterface&   ej_interface,
    hardware_interface::VelocityJointInterface& vj_interface,
    hardware_interface::PositionJointInterface& pj_interface,
    int* joint_mode,
    sensor_msgs::JointStateConstPtr state_msg
  );

  /**
   * \brief Buffers joint state info from Baxter ROS topic
   * \param
   */
  void stateCallback(const sensor_msgs::JointStateConstPtr& msg);

  /**
   * \brief Copy the joint state message into our hardware interface datastructures
   */
  void read( sensor_msgs::JointStateConstPtr &state_msg );

  /**
   * \brief Publish our hardware interface datastructures commands to Baxter hardware
   */
  void write(ros::Duration elapsed_time);

  /**
   * \brief Check if the cuff manual control button is squeezed.
   * \param msg - the state of the end effector cuff
   */
  //void cuffSqueezedCallback(const baxter_core_msgs::DigitalIOStateConstPtr& msg);

  /**
   * \brief This is called when Baxter is disabled, so that we can update the desired positions
   */
  void robotDisabledCallback();

  /**
   * \brief inform the trajectory controller to update its setpoint
   */
  void publishCurrentLocation();
};

} // namespace

#endif

arm_hardware_interface.cpp

#include <zzz_arm_2_hardware/arm_hardware_interface.h>
#include <angles/angles.h>

#include <urdf_parser/urdf_parser.h>

#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>

#include <pluginlib/class_list_macros.h>

#include <zzz_arm_2_control_driver_msgs/joint_msg.h>

namespace zzz_arm_2_control
{
using namespace hardware_interface;
ArmHardwareInterface::ArmHardwareInterface(const std::string &arm_name, double loop_hz)
  : ArmInterface(arm_name, loop_hz),
    cuff_squeezed_previous(false)
{
  // Populate joints in this arm
  /*joint_names_.push_back(arm_name_+"_e0");
  ...*/

  joint_names_.push_back("shoulder_zhuan_joint");
  joint_names_.push_back("upper_arm_joint");
  joint_names_.push_back("fore_arm_joint");
  joint_names_.push_back("hand_wan_joint");
  joint_names_.push_back("hand_zhuan_joint");
  //joint_names_.push_back("finger_1_joint");
  //joint_names_.push_back("finger_2_joint");
  n_dof_ = joint_names_.size();

  // Resize vectors
  joint_position_.resize(n_dof_);
  joint_velocity_.resize(n_dof_);
  joint_effort_.resize(n_dof_);
  joint_position_command_.resize(n_dof_);
  joint_effort_command_.resize(n_dof_);
  joint_velocity_command_.resize(n_dof_);
  //output_msg_.command.resize(n_dof_);
  //output_msg_.names.resize(n_dof_);
  trajectory_command_msg_.joint_names.resize(n_dof_);
  joint_id_to_joint_states_id_.resize(n_dof_);

  for (std::size_t i = 0; i < n_dof_; ++i)
  {
    joint_position_[i] = 0.0;
    joint_velocity_[i] = 0.0;
    joint_effort_[i] = 0.0;
    joint_position_command_[i] = 0.0;
    joint_effort_command_[i] = 0.0;
    joint_velocity_command_[i] = 0.0;
    trajectory_command_msg_.joint_names[i] = joint_names_[i];
  }

  // Set trajectory to have two point
  trajectory_msgs::JointTrajectoryPoint single_pt;
  single_pt.positions.resize(n_dof_);
  single_pt.time_from_start = ros::Duration(0);
  trajectory_command_msg_.points.push_back(single_pt);

  trajectory_msgs::JointTrajectoryPoint single_pt2;
  single_pt2.positions.resize(n_dof_);
  single_pt2.time_from_start = ros::Duration(0.5);
  trajectory_command_msg_.points.push_back(single_pt2);
}

ArmHardwareInterface::~ArmHardwareInterface()
{
}

bool ArmHardwareInterface::init(
  hardware_interface::JointStateInterface&    js_interface,
  hardware_interface::EffortJointInterface&   ej_interface,
  hardware_interface::VelocityJointInterface& vj_interface,
  hardware_interface::PositionJointInterface& pj_interface,
  int* joint_mode,
  sensor_msgs::JointStateConstPtr state_msg)
{
  joint_mode_ = joint_mode;

  for (std::size_t i = 0; i < n_dof_; ++i)
  {
    // Create joint state interface for all joints
    js_interface.registerHandle(hardware_interface::JointStateHandle(
        joint_names_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]));

    // Create position joint interface
    pj_interface.registerHandle(hardware_interface::JointHandle(
        js_interface.getHandle(joint_names_[i]),&joint_position_command_[i]));

    // Create velocity joint interface
    vj_interface.registerHandle(hardware_interface::JointHandle(
        js_interface.getHandle(joint_names_[i]),&joint_velocity_command_[i]));

    // Create effort joint interface
    ej_interface.registerHandle(hardware_interface::JointHandle(
        js_interface.getHandle(joint_names_[i]),&joint_effort_command_[i]));
  }

  // Start publishers
  /*pub_joint_command_ = nh_.advertise<baxter_core_msgs::JointCommand>("/robot/limb/"+arm_name_+
                       "/joint_command",10);*/

  pub_trajectory_command_ = nh_.advertise<trajectory_msgs::JointTrajectory>("/robot/"+arm_name_+
                            "_trajectory_controller/command",10);

  // Start subscribers
  /*cuff_squeezed_sub_ = nh_.subscribe<baxter_core_msgs::DigitalIOState>("/robot/digital_io/" +
                       arm_name_ + "_lower_cuff/state",
                       1, &ArmHardwareInterface::cuffSqueezedCallback, this);
*/

  zzz_fake_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/move_group/fake_controller_joint_states", 1);
  joint_motor_pub_= nh_.advertise<zzz_arm_2_control_driver_msgs::joint_msg>("/arm_motors", 1000);
  // Make a mapping of joint names to indexes in the joint_states message
  for (std::size_t i = 0; i < n_dof_; ++i)
  {
    std::vector<std::string>::const_iterator iter = std::find(state_msg->name.begin(), state_msg->name.end(), joint_names_[i]);
    size_t joint_states_id = std::distance(state_msg->name.begin(), iter);
    if(joint_states_id == state_msg->name.size())
    {
      ROS_ERROR_STREAM_NAMED(arm_name_,"Unable to find joint " << i << " named " << joint_names_[i] << " in joint state message");
    }

    joint_id_to_joint_states_id_[i] = joint_states_id;

    ROS_DEBUG_STREAM_NAMED("arm_hardware_interface","Found joint " << i << " at " << joint_states_id << " named " << joint_names_[i]);
  }

  // Set the initial command values based on current state
  for (std::size_t i = 0; i < n_dof_; ++i)
  {
    joint_position_command_[i] = state_msg->position[joint_id_to_joint_states_id_[i]];

    // Pre-load the joint names into the output messages just once
    //output_msg_.names[i] = joint_names_[i];
  }

  // Position joint limits interface
 /* std::vector<std::string> cmd_handle_names = pj_interface.getNames();
  for (size_t i = 0; i < n_dof_; ++i)
  {
    const std::string name = cmd_handle_names[i];
    JointHandle cmd_handle = pj_interface.getHandle(name);

    using namespace joint_limits_interface;
    boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(name);
    JointLimits limits;
    SoftJointLimits soft_limits;
    if (!getJointLimits(urdf_joint, limits) || !getSoftJointLimits(urdf_joint, soft_limits))
    {
      ROS_WARN_STREAM("Joint limits won't be enforced for joint '" << name << "'.");
    }
    else
    {
      jnt_limits_interface_.registerHandle(
          PositionJointSoftLimitsHandle(cmd_handle, limits, soft_limits));

      ROS_DEBUG_STREAM("Joint limits will be enforced for joint '" << name << "'.");
    }
  }*/
  // PID controllers
  pids_.resize(n_dof_);
  for (size_t i = 0; i < n_dof_; ++i)
  {
    ros::NodeHandle joint_nh(nh_, "gains/" + joint_names_[i]);

    if (!pids_[i].init(joint_nh))
    {
      ROS_WARN_STREAM("PID data err 'gains/" << joint_names_[i] << "'.");

      //return false;
    }
  }
  ROS_INFO_NAMED(arm_name_, "Loaded zzz_arm_2_hardware_interface.");
  return true;
}

void ArmHardwareInterface::read( sensor_msgs::JointStateConstPtr &state_msg )
{
  // Copy state message to our datastructures
  //ROS_INFO_STREAM_NAMED("arm_hardware_interface","n_dof_="<<n_dof_);
  for (std::size_t i = 0; i < n_dof_; ++i)
  {

    //ROS_INFO_STREAM_NAMED("arm_hardware_interface","Joint " << i << "("<< joint_names_[i] << ") -> " << joint_id_to_joint_states_id_[i] << " position= " << state_msg->position[joint_id_to_joint_states_id_[i]]);
    std::size_t sp=state_msg->position.size();
    std::size_t sv=state_msg->velocity.size();
    std::size_t se=state_msg->effort.size();
    joint_position_[i] = sp>joint_id_to_joint_states_id_[i]?state_msg->position[joint_id_to_joint_states_id_[i]]:0.0;
    joint_velocity_[i] = sv>joint_id_to_joint_states_id_[i]?state_msg->velocity[joint_id_to_joint_states_id_[i]]:0.0;
    joint_effort_[i] = se>joint_id_to_joint_states_id_[i]?state_msg->effort[joint_id_to_joint_states_id_[i]]:0.0;
  }
}

void ArmHardwareInterface::write(ros::Duration elapsed_time)
{
  // Enforce joint limits
  //jnt_limits_interface_.enforceLimits(elapsed_time);
  zzz_arm_2_control_driver_msgs::joint_msg joint_motor_msg;

  // Compute and send commands
  sensor_msgs::JointState joint_state_msg;
  joint_state_msg.name.resize(n_dof_);
  joint_state_msg.position.resize(n_dof_);
  joint_state_msg.header.stamp = ros::Time::now();
  for (size_t i = 0; i < n_dof_; ++i)
  {
    //const double error = joint_position_command_[i] - joint_position_[i];
    //const double effort = pids_[i].computeCommand(error, elapsed_time);
    joint_motor_msg.id=i;
    joint_motor_msg.r=joint_position_command_[i];
    joint_motor_pub_.publish(joint_motor_msg);

    //joint_state_msg.name[i] =joint_names_[i];
    //joint_state_msg.position[i] = joint_position_command_[i] ;


    //sim_joints_[i]->SetForce(0u, effort);
  }
  //zzz_fake_joint_pub_.publish(joint_state_msg);
  ros::spinOnce();
  //ROS_INFO("run write"  );

  // Send commands to baxter in different modes

  /*switch (*joint_mode_)
  {
    case hardware_interface::MODE_POSITION:
      output_msg_.command = joint_position_command_;
      output_msg_.mode = baxter_core_msgs::JointCommand::POSITION_MODE;
      break;
    case hardware_interface::MODE_VELOCITY:
      output_msg_.command = joint_velocity_command_;
      output_msg_.mode = baxter_core_msgs::JointCommand::VELOCITY_MODE;
      break;
    case hardware_interface::MODE_EFFORT:
      output_msg_.command = joint_effort_command_;
      output_msg_.mode = baxter_core_msgs::JointCommand::TORQUE_MODE;
      break;
  }*/

  // Publish
  //pub_joint_command_.publish(output_msg_);
}
/*
void ArmHardwareInterface::cuffSqueezedCallback(const baxter_core_msgs::DigitalIOStateConstPtr& msg)
{
  // Check if button is pressed
  if( msg->state == 1 )
  {
    cuff_squeezed_previous = true;
  }
  else  // button not pressed
  {
    if ( cuff_squeezed_previous )
    {

      publishCurrentLocation();
    }
    cuff_squeezed_previous = false;
  }
}
*/
void ArmHardwareInterface::publishCurrentLocation()
{
  // Publish this new trajectory just once, on cuff release
  ROS_INFO_STREAM_NAMED(arm_name_, "Sent updated trajectory to trajectory controller");

  // Update the trajectory message with the current positions
  for (std::size_t i = 0; i < n_dof_; ++i)
  {
    trajectory_command_msg_.points[0].positions[i] = joint_position_[i];
    trajectory_command_msg_.points[1].positions[i] = joint_position_[i];
  }

  // Send a trajectory
  pub_trajectory_command_.publish(trajectory_command_msg_);
}

void ArmHardwareInterface::robotDisabledCallback()
{
  publishCurrentLocation();
}

} // namespace

zzz_arm_2_hardware_interface.h

#ifndef BAXTER_CONTROL__BAXTER_HARDWARE_INTERFACE_
#define BAXTER_CONTROL__BAXTER_HARDWARE_INTERFACE_

// Boost
#include <boost/shared_ptr.hpp>

// ROS
#include <ros/ros.h>

// ros_control
#include <controller_manager/controller_manager.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
//#include <hardware_interface/joint_mode_interface.h>
#include <hardware_interface/robot_hw.h>

// Baxter
//#include <zzz_arm_2_hardware/baxter_utilities.h>
#include <zzz_arm_2_hardware/arm_interface.h>
#include <zzz_arm_2_hardware/arm_hardware_interface.h>
//#include <zzz_arm_2_hardware/arm_simulator_interface.h>

namespace zzz_arm_2_control
{

static const double NUM_BAXTER_JOINTS = 5;

class ZZZArm2HardwareInterface : public hardware_interface::RobotHW
{
private:

  // Node Handles
  ros::NodeHandle nh_; // no namespace

  // Timing
  ros::Duration control_period_;
  ros::Time last_sim_time_ros_;
  ros::Duration elapsed_time_;
  double loop_hz_;

  // Interfaces
  hardware_interface::JointStateInterface    js_interface_;
  //hardware_interface::JointModeInterface     jm_interface_;
  hardware_interface::EffortJointInterface   ej_interface_;
  hardware_interface::VelocityJointInterface vj_interface_;
  hardware_interface::PositionJointInterface pj_interface_;

  // baxter helper
  //baxter_control::BaxterUtilities baxter_util_;

  // sub-hardware interfaces
  ArmInterfacePtr right_arm_hw_;
  //ArmInterfacePtr left_arm_hw_;

  boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;

  ros::Timer non_realtime_loop_;

  bool in_simulation_;

  // Which joint mode are we in
  int joint_mode_;

  // Buffer of joint states to share between arms
  sensor_msgs::JointStateConstPtr state_msg_;
  ros::Time state_msg_timestamp_;

  // Subscriber
  ros::Subscriber sub_joint_state_;

public:

  /**
   * \brief Constructor/Descructor
   */
  ZZZArm2HardwareInterface(bool in_simulation);
  ~ZZZArm2HardwareInterface();

  /**
   * \brief Checks if the state message from Baxter is out of date
   * \return true if expired
   */
  bool stateExpired();

  void stateCallback(const sensor_msgs::JointStateConstPtr& msg);

  void update(const ros::TimerEvent& e);

};

} // namespace

#endif

zzz_arm_2_hardware_interface.cpp

#include <zzz_arm_2_hardware/zzz_arm_2_hardware_interface.h>

namespace zzz_arm_2_control
{

ZZZArm2HardwareInterface::ZZZArm2HardwareInterface(bool in_simulation)
  : in_simulation_(in_simulation),
    joint_mode_(1),
    loop_hz_(75)//controller update speed
{
  if( in_simulation_ )
  {
    ROS_INFO_STREAM_NAMED("hardware_interface","Running in simulation mode");
    //right_arm_hw_.reset(new zzz_arm_2_control::ArmSimulatorInterface("right",loop_hz_));
    //left_arm_hw_.reset(new zzz_arm_2_control::ArmSimulatorInterface("left",loop_hz_));
  }
  else
  {
    ROS_INFO_STREAM_NAMED("hardware_interface","Running in hardware mode");
    right_arm_hw_.reset(new zzz_arm_2_control::ArmHardwareInterface("right",loop_hz_));
    //left_arm_hw_.reset(new zzz_arm_2_control::ArmHardwareInterface("left",loop_hz_));
  }

  // Set the joint mode interface data
  //jm_interface_.registerHandle(hardware_interface::JointModeHandle("joint_mode", &joint_mode_));

  // Start the shared joint state subscriber
  sub_joint_state_ = nh_.subscribe<sensor_msgs::JointState>("/robot/joint_states", 1,
                     &ZZZArm2HardwareInterface::stateCallback, this);

  // Wait for first state message to be recieved if we are not in simulation
  if (!in_simulation_)
  {
    // Loop until we find a joint_state message from Baxter
    do
    {
      // Loop until we get our first joint_state message
      while(ros::ok() && state_msg_timestamp_.toSec() == 0)
      {
        ROS_INFO_STREAM_NAMED("hardware_interface","Waiting for first state message to be recieved");
        ros::spinOnce();
        ros::Duration(0.25).sleep();
      }
    } while (state_msg_->name.size() < NUM_BAXTER_JOINTS);
  }

  // Initialize right arm
  right_arm_hw_->init(js_interface_, ej_interface_, vj_interface_, pj_interface_, &joint_mode_, state_msg_);
  //left_arm_hw_->init(js_interface_, ej_interface_, vj_interface_, pj_interface_, &joint_mode_, state_msg_);

  // Register interfaces
  registerInterface(&js_interface_);
  //registerInterface(&jm_interface_);
  registerInterface(&ej_interface_);
  registerInterface(&vj_interface_);
  registerInterface(&pj_interface_);

  // Enable baxter
  /*bool enabled = false;
  while(!enabled)
  {
    if( !baxter_util_.enableBaxter() )
    {
      ROS_WARN_STREAM_NAMED("hardware_interface","Unable to enable Baxter, retrying...");
      ros::Duration(0.5).sleep();
      ros::spinOnce();
    }
    else
    {
      enabled = true;
    }
  }
*/
  // Set callback for Baxter being disabled
  //baxter_util_.setDisabledCallback(boost::bind( &ArmInterface::robotDisabledCallback, right_arm_hw_ ));
  //baxter_util_.setDisabledCallback(boost::bind( &ArmInterface::robotDisabledCallback,  left_arm_hw_ ));

  // Create the controller manager
  ROS_DEBUG_STREAM_NAMED("hardware_interface","Loading controller_manager");
  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));

  ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
  non_realtime_loop_ = nh_.createTimer(update_freq, &ZZZArm2HardwareInterface::update, this);

  ROS_INFO_NAMED("hardware_interface", "Loaded baxter_hardware_interface.");
}

ZZZArm2HardwareInterface::~ZZZArm2HardwareInterface()
{
  //baxter_util_.disableBaxter();
}

bool ZZZArm2HardwareInterface::stateExpired()
{
  // Check that we have a non-expired state message
  // \todo lower the expiration duration
  if( ros::Time::now() > state_msg_timestamp_ + ros::Duration(STATE_EXPIRED_TIMEOUT)) // check that the message timestamp is no older than 1 second
  {

    ROS_WARN_STREAM_THROTTLE_NAMED(1,"hardware_interface","State expired. Last recieved state " << (ros::Time::now() - state_msg_timestamp_).toSec() << " seconds ago." );
    return true;
  }
  return false;
}

void ZZZArm2HardwareInterface::stateCallback(const sensor_msgs::JointStateConstPtr& msg)
{
  //ROS_INFO("size=%d",msg->name.size());
  // Check if this message has the correct number of joints
  if( msg->name.size() < NUM_BAXTER_JOINTS )
  {
    return;
  }

  // Copy the latest message into a buffer
  state_msg_ = msg;
  state_msg_timestamp_ = ros::Time::now();
}

void ZZZArm2HardwareInterface::update(const ros::TimerEvent& e)
{
  //ROS_INFO("update");
  // Check if state msg from Baxter is expired
  if( !in_simulation_ && stateExpired() )
    return;

  elapsed_time_ = ros::Duration(e.current_real - e.last_real);

  // Input
  right_arm_hw_->read(state_msg_);
  //left_arm_hw_->read(state_msg_);

  // Control
  controller_manager_->update(ros::Time::now(), elapsed_time_);

  // Output
  right_arm_hw_->write(elapsed_time_);
  //left_arm_hw_->write(elapsed_time_);
}

} // namespace

int main(int argc, char** argv)
{
  ROS_INFO_STREAM_NAMED("zzz_arm_2_hardware_interface","Starting hardware interface...");

  ros::init(argc, argv, "zzz_arm_2_hardware_interface");

  // Allow the action server to recieve and send ros messages
  ros::AsyncSpinner spinner(4);
  spinner.start();

  ros::NodeHandle nh;

  bool in_simulation = false;

  // Parse command line arguments
  for (std::size_t i = 0; i < argc; ++i)
  {
    if( std::string(argv[i]).compare("--simulation") == 0 )
    {
      ROS_INFO_STREAM_NAMED("main","zzz arm Hardware Interface in simulation mode");
      in_simulation = true;
    }
  }

  zzz_arm_2_control::ZZZArm2HardwareInterface zzzarm(in_simulation);

  //ros::spin();//error
  ros::waitForShutdown();
  ROS_INFO_STREAM_NAMED("hardware_interface","Shutting down.");

  return 0;
}

ros_control 使用的yaml

zzz_arm:
    zzz_arm_controller:
      type: position_controllers/JointTrajectoryController
      joints:
        - shoulder_zhuan_joint
        - upper_arm_joint
        - fore_arm_joint
        - hand_wan_joint
        - hand_zhuan_joint

      constraints:
        goal_time: &goal_time_constraint 10.0
        shoulder_zhuan_joint:
          goal: &goal_pos_constraint 0.5
        upper_arm_joint:
          goal: *goal_pos_constraint
        fore_arm_joint:
          goal: *goal_pos_constraint
        hand_wan_joint:
          goal: *goal_pos_constraint
        hand_zhuan_joint:
          goal: *goal_pos_constraint

    zzz_grapper_controller:
      type: position_controllers/JointTrajectoryController
      joints:
        - finger_1_joint
        - finger_2_joint

      constraints:
        goal_time: *goal_time_constraint
        finger_1_joint:
          goal: *goal_pos_constraint
        finger_2_joint:
          goal: *goal_pos_constraint

# Publish all joint states -----------------------------------
    joint_state_controller:
      type: joint_state_controller/JointStateController
      publish_rate: 50



    gains:
        shoulder_zhuan_joint:   {p: 3.0, i: 0.01, d: 1.1, i_clamp: 1.0}
        upper_arm_joint:      {p: 3.0, i: 0.01, d: 1.1, i_clamp: 1.0}
        fore_arm_joint:     {p: 3.0, i: 0.01, d: 1.1, i_clamp: 1.0}
        hand_wan_joint:       {p: 3.0, i: 0.01, d: 1.1, i_clamp: 1.0}
        hand_zhuan_joint:           {p: 3.0, i: 0.01, d: 1.1, i_clamp: 1.0}
        finger_1_joint:       {p: 3.0, i: 0.01, d: 1.0, i_clamp: 1.0}
        finger_2_joint:       {p: 3.0, i: 0.01, d: 1.0, i_clamp: 1.0}

moveit使用的yaml

controller_list:
  - name: zzz_arm/zzz_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - shoulder_zhuan_joint
      - upper_arm_joint
      - fore_arm_joint
      - hand_wan_joint
      - hand_zhuan_joint
  - name: zzz_arm/zzz_grapper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_1_joint
      - finger_2_joint

launch 调用controller_manager 来启动 controllers

   

<!-- Load the default controllers -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen"  args="zzz_arm_controller zzz_grapper_controller
                                 joint_state_controller
                        " />

可视化查看 controllers情况
sudo apt-get install ros-kinetic-rqt-controller-manager

zzz@zzz-ubuntu:~/rostest$ rosrun rqt_controller_manager rqt_controller_manager
qt_gui_main() found no plugin matching "rqt_controller_manager

问题出在 qt 的缓存没有更新安装插件。解决办法：

    $ rm ~/.config/ros.org/rqt_gui.ini  
    $ rqt



错误
Spinner Monitor: single-treaded spinner after multi-threaded ones

原因

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
//ros::spin();//error写成这样不知到作者是怎么编译成功的。反正我这里是过不去。

解决
http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown();

controller spawner coudn't find the expected controller_manager ROS interface
注意ns设置

zzz_arm_2_control_driver包下创建的joint_msg，包内节点使用没任何问题。
在另外的包引用
#include <zzz_arm_2_control_driver/joint_msg.h>
出现错误
ros fatal error: xxx/xxx.h: No such file or directory
以为是缺少配置

<build_depend>zzz_arm_2_control_driver</build_depend>
<run_depend>zzz_arm_2_control_driver</run_depend>

find_package(catkin REQUIRED COMPONENTS 
zzz_arm_2_control_driver
)

添加了又出现新的错误
ros The library is neither a target nor built...
did you find_package() it before the subdirectory containing its code is included?
半天无解




只好新建一个只包含joint_msg 的包zzz_arm_2_control_driver_msgs
其它包引用

<build_depend>zzz_arm_2_control_driver_msgs</build_depend>
<run_depend>zzz_arm_2_control_driver_msgs</run_depend>

find_package(catkin REQUIRED COMPONENTS
zzz_arm_2_control_driver_msgs
)

#include <zzz_arm_2_control_driver_msgs/joint_msg.h>

编译通过，水平菜真是不知道哪里有问题。
以后msg单独建包编译吧，有问题也能一目了然。



usb串口又忘了权限报错
terminate called after throwing an instance of 'boost::exception_detail::clone_impl
解决
sudo chmod 666 /dev/ttyUSB0




controller is taking too long to execute trajectory
舵机通信速度会影响 controller 的执行
controller update速度过快或者与舵机通信（joint_states 更新）速度过慢
controller就认为机器关节运动速度达不到要求（太慢甚至会认为没有执行动作）导致规划轨迹执行失败。


update速率根据实际情况计算(以下为不包括延时处理及等待时间的理论值，实际测试controller update在10hz左右为好)
波特率115200,每秒有11520字节被传送
通信协议
发送
位置控制#000P1500T1000!（15byte）
位置回读#000PRAD!（9byte）
接收
回读信息#000P1500!（10byte）
发送字节数计算 15+9=24 ，24×6（个舵机）=144byte
带宽全占满时是11520/144=80hz
串口joint_states更新频率controller update频率都应该匹配这个80hz

（考虑位置控制不是每次都发送因此joint_states更新频率可以适当高一点（100），考虑到ros消息非实时性controller update可以适当低一点（75））


高手用ros_control驱动 baxter
我用ros_control驱动低端的舵机手臂，是不是有杀鸡用牛刀的感觉？

好吧，还是老话，走自己的路让别人说去吧。


hello,baxter!