ROS-I Interface Layer 源码分析：Joint

JointTrajectoryInterface类的功能：将ROS端的joint trajectories转发给机器人控制器。

namespace industrial_robot_client
{
namespace joint_trajectory_interface
{

  using industrial::smpl_msg_connection::SmplMsgConnection;
  using industrial::tcp_client::TcpClient;
  using industrial::joint_traj_pt_message::JointTrajPtMessage;
  namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts;

class JointTrajectoryInterface
{

public:

  JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0) {};

  virtual bool init(std::string default_ip = "", int default_port = StandardSocketPorts::MOTION);
  virtual bool init(SmplMsgConnection* connection);
  virtual bool init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
                    const std::map<std::string, double> &velocity_limits = std::map<std::string, double>());

  virtual ~JointTrajectoryInterface();

  virtual void run() { ros::spin(); }

protected:

  virtual void trajectoryStop();
  virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<JointTrajPtMessage>* msgs);
  virtual bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out)
  {
    *pt_out = pt_in;  // by default, no transform is applied
    return true;
  }
  virtual bool select(const std::vector<std::string>& ros_joint_names, const trajectory_msgs::JointTrajectoryPoint& ros_pt,
                      const std::vector<std::string>& rbt_joint_names, trajectory_msgs::JointTrajectoryPoint* rbt_pt);
  virtual bool calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration);
  virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity);
  virtual bool calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration);
  virtual bool send_to_robot(const std::vector<JointTrajPtMessage>& messages)=0;

  virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
  virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req,
                                    industrial_msgs::StopMotion::Response &res);
  virtual bool is_valid(const trajectory_msgs::JointTrajectory &traj);
  virtual void jointStateCB(const sensor_msgs::JointStateConstPtr &msg);


  TcpClient default_tcp_connection_;
  ros::NodeHandle node_;
  SmplMsgConnection* connection_;
  ros::Subscriber sub_cur_pos_; 
  ros::Subscriber sub_joint_trajectory_;
  ros::ServiceServer srv_joint_trajectory_; 
  ros::ServiceServer srv_stop_motion_;
  std::vector<std::string> all_joint_names_;
  double default_joint_pos_;
  double default_vel_ratio_;
  double default_duration_;
  std::map<std::string, double> joint_vel_limits_; 
  sensor_msgs::JointState cur_joint_pos_; 


private:
  static JointTrajPtMessage create_message(int seq, std::vector<double> joint_pos, double velocity, double duration);
  bool jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
                         industrial_msgs::CmdJointTrajectory::Response &res);
};

} //joint_trajectory_interface
} //industrial_robot_client

JointTrajectoryInterface类包含下列成员：

成员变量	类型	含义
default_tcp_connection_	TcpClient	TCP客户端
node_	ros::NodeHandle	ROS节点句柄
connection_	SmplMsgConnection*	指向当前使用的connection
sub_cur_pos_	ros::Subscriber	订阅joint_state话题
sub_joint_trajectory_	ros::Subscriber	订阅joint_trajectory话题
srv_joint_trajectory_	ros::ServiceServer	提供joint_trajectory服务
srv_stop_motion_	ros::ServiceServer	提供stop_motion服务
all_joint_names_	std::vector<std::string>	关节名称
default_joint_pos_	double	dumy关节的默认位置：0
default_vel_ratio_	double	默认速度水平：0.1
default_duration_	double	默认轨迹点运动时间：10.0
joint_vel_limits_	std::map<std::string, double>	从URDF中获取的关节速度限制
cur_joint_pos_	sensor_msgs::JointState	上一次收到的joint_state

先看初始化方法init

bool JointTrajectoryInterface::init(std::string default_ip, int default_port)
{
  std::string ip;
  int port;

  ros::param::param<std::string>("robot_ip_address", ip, default_ip);
  ros::param::param<int>("~port", port, default_port);

  // check for valid parameter values
  if (ip.empty())
  {
    ROS_ERROR("No valid robot IP address found.  Please set ROS 'robot_ip_address' param");
    return false;
  }
  if (port <= 0)
  {
    ROS_ERROR("No valid robot IP port found.  Please set ROS '~port' param");
    return false;
  }

  char* ip_addr = strdup(ip.c_str());  // connection.init() requires "char*", not "const char*"
  ROS_INFO("Joint Trajectory Interface connecting to IP address: '%s:%d'", ip_addr, port);
  default_tcp_connection_.init(ip_addr, port);
  free(ip_addr);

  return init(&default_tcp_connection_);
}

初始化过程：首先会检查ROS参数服务器上有没有配置robot_ip_address或~port，如果有则覆盖传进来的参数default_ip或default_port，如果ip或端口号参数无效则返回。接着用IP地址和端口号初始化TCP客户端。最后调用重载的init:

bool JointTrajectoryInterface::init(SmplMsgConnection* connection)
{
  std::vector<std::string> joint_names;
  if (!getJointNames("controller_joint_names", "robot_description", joint_names))
  {
    ROS_ERROR("Failed to initialize joint_names.  Aborting");
    return false;
  }

  return init(connection, joint_names);
}

这个init用于获取ROS和控制器想要交互数据的关节名称，getJointNames首先会检查/controller_joint_names这个ROS参数是否有配置joint_name，如果找不到则从/robot_description参数指定的urdf文件中生成joint_name。找到后会将关节名称写入joint_names这个string类型的vector。最后调用另一个重载的init:

bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
                                    const std::map<std::string, double> &velocity_limits)
{
  this->connection_ = connection;
  this->all_joint_names_ = joint_names;
  this->joint_vel_limits_ = velocity_limits;
  connection_->makeConnect();

  // try to read velocity limits from URDF, if none specified
  if (joint_vel_limits_.empty() && !industrial_utils::param::getJointVelocityLimits("robot_description", joint_vel_limits_))
    ROS_WARN("Unable to read velocity limits from 'robot_description' param.  Velocity validation disabled.");

  this->srv_stop_motion_ = this->node_.advertiseService("stop_motion", &JointTrajectoryInterface::stopMotionCB, this);
  this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this);
  this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this);
  this->sub_cur_pos_ = this->node_.subscribe("joint_states", 1, &JointTrajectoryInterface::jointStateCB, this);

  return true;
}

首先会保存当前的连接，关节名称，速度限制，然后与服务器（机器人控制器）建立连接。接着从urdf中读取速度限制，最后初始化本节点的ROS话题订阅和ROS服务。订阅的话题回调有2个：jointTrajectoryCB和jointStateCB，分别指定了接收到joint_path_command和joint_states话题后执行的函数。提供的服务也有2个：jointTrajectoryCB和stop_motion，分别用于接收joint_path_command请求（传输轨迹至控制器）和接收stop_motion请求（用于停止机器人运动）。

jointTrajectoryCB话题回调函数

void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
{
  ROS_INFO("Receiving joint trajectory message");

  // check for STOP command
  if (msg->points.empty())
  {
    ROS_INFO("Empty trajectory received, canceling current trajectory");
    trajectoryStop();
    return;
  }

  // convert trajectory into robot-format
  std::vector<JointTrajPtMessage> robot_msgs;
  if (!trajectory_to_msgs(msg, &robot_msgs))
    return;

  // send command messages to robot
  send_to_robot(robot_msgs);
}

jointTrajectoryCB话题回调中，先检查接收到的msg轨迹points是否为空，JointTrajectory的消息结构是这样的：

[trajectory_msgs/JointTrajectory]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start

如果消息为空，则调用trajectoryStop让机器人停止运动，如果有轨迹点，则调用trajectory_to_msgs将ROS的消息转化未JointTrajPtMessage消息，最后调用虚方法send_to_robot将JointTrajPtMessage消息发送给机器人控制器。之所以是虚方法，是为了实现两种模式的轨迹传输：streaming模式和download模式。这两种模式后面会分别介绍。

下面看下trajectory_to_msgs做了什么：

bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector<JointTrajPtMessage>* msgs)
{
  msgs->clear();

  // check for valid trajectory
  if (!is_valid(*traj))
    return false;

  for (size_t i=0; i<traj->points.size(); ++i)
  {
    ros_JointTrajPt rbt_pt, xform_pt;
    double vel, duration;

    // select / reorder joints for sending to robot
    if (!select(traj->joint_names, traj->points[i], this->all_joint_names_, &rbt_pt))
      return false;

    // transform point data (e.g. for joint-coupling)
    if (!transform(rbt_pt, &xform_pt))
      return false;

    // reduce velocity to a single scalar, for robot command
    if (!calc_speed(xform_pt, &vel, &duration))
      return false;

    JointTrajPtMessage msg = create_message(i, xform_pt.positions, vel, duration);
    msgs->push_back(msg);
  }

  return true;
}

首先检查JointTrajectory消息是否有效，如轨迹中的position是否为空，velocity是否超过最大限制，时间戳是否有效（对于第一个点后的时间应该均大于0）。接着对于这一条轨迹点遍历，做如下的工作：
调用select方法选择需要和控制器交互的joint名称，即检查和控制器交互的joint名称是否包含在ROS消息中。接着调用transform以考虑关节耦合，对于机械臂通常无须考虑。接着调用calc_speed计算速度水平和时间，vel中保存了关节速度/最大关节速度中的最大值，时间间隔duration则用当前轨迹点时间减去上一个轨迹点时间得到。接着调用create_message创建JointTrajPtMessage消息，也就是用处理过的ROS消息填充内部封装了JointTrajPt数据结构的JointTrajPtMessage。最后将消息添加至msgs，完成循环体的一次执行。

jointTrajectoryCB服务回调函数

bool JointTrajectoryInterface::jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
                                                 industrial_msgs::CmdJointTrajectory::Response &res)
{
  trajectory_msgs::JointTrajectoryPtr traj_ptr(new trajectory_msgs::JointTrajectory);
  *traj_ptr = req.trajectory;  // copy message data
  this->jointTrajectoryCB(traj_ptr);

  // no success/fail result from jointTrajectoryCB.  Assume success.
  res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;

  return true;  // always return true.  To distinguish between call-failed and service-unavailable.
}

有了上面的jointTrajectoryCB topic回调，这里的jointTrajectoryCB服务回调就可以直接利用它实现了。首先将收到的request中的trajectory提取出来（见下面的服务定义），接着调用topic回调，最后填充服务返回码即可。注意到这里是直接返回成功的，即没有考虑发送失败情况。对照该服务类型CmdJointTrajectory定义，其中也提到，返回success并不一定100%保证轨迹发送成功：

# Send a JointTrajectory command to the robot.
#   - duplicates functionality of the joint_path_command topic
#   - provides a response-code to verify command was received
#   - returns when trajectory is sent to robot, not when motion completed
#   - return code may NOT indicate successful transfer to robot

trajectory_msgs/JointTrajectory trajectory
---
industrial_msgs/ServiceReturnCode code

jointStateCB话题回调函数

void JointTrajectoryInterface::jointStateCB(const sensor_msgs::JointStateConstPtr &msg)
{
  this->cur_joint_pos_ = *msg;
}

这个函数很简单，直接将收到的JointState消息保存在了成员变量内。并未用于任何处理，说明处理程序留给用户的子类实现的。

stopMotionCB服务回调函数

bool JointTrajectoryInterface::stopMotionCB(industrial_msgs::StopMotion::Request &req,
                                            industrial_msgs::StopMotion::Response &res)
{
  trajectoryStop();

  // no success/fail result from trajectoryStop.  Assume success.
  res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS;

  return true;  // always return true.  To distinguish between call-failed and service-unavailable.
}

StopMotion服务的定义如下：

# Stops current robot motion.  Motion resumed by using start_motion service
# or by sending a new motion command

---
industrial_msgs/ServiceReturnCode code

接收到stopMotion请求后，stopMotionCB会调用trajectoryStop发送停止命令给机器人控制器，那么最后看一下trajectoryStop的代码：

trajectoryStop

void JointTrajectoryInterface::trajectoryStop()
{
  JointTrajPtMessage jMsg;
  SimpleMessage msg, reply;

  ROS_INFO("Joint trajectory handler: entering stopping state");
  jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
  jMsg.toRequest(msg);
  ROS_DEBUG("Sending stop command");
  this->connection_->sendAndReceiveMsg(msg, reply);
}

首先初始化JointTrajPtMessage，将其中封装的数据结构JointTrajPt中的序号配置为STOP_TRAJECTORY，再将JointTrajPtMessage消息转为回复类型的SimpleMessage消息，最后调用连接的发送并接收方法，完成停止命令的发送。

以上是对于JointTrajectoryInterface类的分析，接下来的文章将分析一下其子类JointTrajectoryStreamer和JointTrajectoryDownloader，它们分别实现了轨迹点的streaming（流）和download（下载）传输模式。