ROS-I simple_message 源码分析：SimpleMessage

概述

SimpleMessage类定义了ROS的驱动层（也就是我们写的client node）和机器人控制器之间的通信协议，说白了就是定义了交互消息的数据格式。这个协议满足了ROS-I定义的如下4个要求：

格式要足够的简单，协议代码能够在ROS端和控制器端共享，当然这个是针对支持C/C++的控制器。对于不支持C/C++的控制器，则必须要保证能够使用机器人编程语言来处理消息。也就是说如果协议太复杂的话，有限的控制器资源会难以处理。
要允许数据流，类似ROS topic
要允许数据回复，类似ROS service
协议不会封装版本信息，也就是说开发者要自己负责统一ROS端和控制器端之间的协议版本

协议格式

SimpleMessage协议定义了消息的前缀（PREFIX）、消息头（HEADER）和消息体（BODY）

PREFIX
(长度1个字节)
LENGTH域，int数据类型，代表(HEADER + DATA)的字节长度
HEADER
(长度3个字节)
MSG_TYPE域，int数据类型，代表了消息的类型，包括标准消息和厂商自定义的消息类型
COMM_TYPE域，int数据类型，代表了通信的类型
REPLY CODE域，int数据类型，代表回复代号，仅在服务回复时有效
BODY
(变长)
DATA域，ByteArray数据类型，代表由消息类型和服务类型决定的可变长度的数据

源代码

对着以上的定义，我们来看看源代码长的什么样：

namespace industrial
{

namespace simple_message
{

namespace StandardMsgTypes
{
  enum StandardMsgType
  {
   INVALID = 0,
   PING = 1,

   JOINT_POSITION = 10,
   JOINT = 10, 
   READ_INPUT = 20,
   WRITE_OUTPUT = 21,

   JOINT_TRAJ_PT = 11,  //Joint trajectory point message (typically for streaming)
   JOINT_TRAJ = 12,   //Joint trajectory message (typically for trajectory downloading)
   STATUS = 13,                //Robot status message (for reporting the robot state)
   JOINT_TRAJ_PT_FULL = 14,  // Joint trajectory point message (all message fields)
   JOINT_FEEDBACK = 15,      // Feedback of joint pos/vel/accel


   SWRI_MSG_BEGIN     = 1000,
   UR_MSG_BEGIN       = 1100,
   ADEPT_MSG_BEGIN    = 1200,
   ABB_MSG_BEGIN      = 1300,
   FANUC_MSG_BEGIN    = 1400,
   MOTOMAN_MSG_BEGIN  = 2000
  };
}
typedef StandardMsgTypes::StandardMsgType StandardMsgType;


namespace CommTypes
{
  enum CommType
  {
   INVALID = 0,
   TOPIC = 1,
   SERVICE_REQUEST = 2,
   SERVICE_REPLY = 3
  };
}
typedef CommTypes::CommType CommType;

namespace ReplyTypes
{
  enum ReplyType
  {
 INVALID = 0,
 SUCCESS = 1,
 FAILURE = 2
  };
}
typedef ReplyTypes::ReplyType ReplyType;


class SimpleMessage
{

public:
  SimpleMessage();
  ~SimpleMessage(void);
  bool init(int msgType, int commType, int replyCode);
  bool init(industrial::byte_array::ByteArray & msg);
  void toByteArray(industrial::byte_array::ByteArray & msg);
  static unsigned int getHeaderSize() { return SimpleMessage::HEADER_SIZE; };
  static unsigned int getLengthSize() { return SimpleMessage::LENGTH_SIZE; };
  int getMessageType() {return this->message_type_;};
  int getCommType() {return this->comm_type_;};
  int getReplyCode() {return this->reply_code_;};
  int getMsgLength() {return this->getHeaderSize() + this->data_.getBufferSize();};
  int getDataLength() {return this->data_.getBufferSize();};
  industrial::byte_array::ByteArray & getData() {return this->data_;};
  bool validateMessage();

private:
  industrial::shared_types::shared_int message_type_;
  industrial::shared_types::shared_int comm_type_;
  industrial::shared_types::shared_int reply_code_;
  industrial::byte_array::ByteArray data_;
  static const unsigned int HEADER_SIZE = sizeof(industrial::shared_types::shared_int) +
        sizeof(industrial::shared_types::shared_int) +
        sizeof(industrial::shared_types::shared_int);
  static const unsigned int LENGTH_SIZE = sizeof(industrial::shared_types::shared_int);
  void setMessageType(int msgType) {this->message_type_ = msgType;};
  void setCommType(int commType) {this->comm_type_ = commType;};
  void setReplyCode(int replyCode) {this->reply_code_ = replyCode;};
  void setData(industrial::byte_array::ByteArray & data);
};

}//namespace simple_message
}//namespace industrial

这个头文件在定义SimpleMessage类之前，定义了一些枚举类型，包括：

StandardMsgType
代表了标准消息的ID号，所有的控制器应当支持这些消息类型，以及比如ABB，UR这些机器人厂商定义的消息类型，厂商可以定义多达100种消息类型。标准消息类型包括16种（不包括vendor specific的）：
INVALID，PING，JOINT_POSITION，JOINT，READ_INPUT，WRITE_OUTPUT，JOINT_TRAJ_PT，JOINT_TRAJ，STATUS，JOINT_TRAJ_PT_FULL，JOINT_FEEDBACK
CommType
代表通信类型，包含INVALID，TOPIC，SERVICE_REQUEST，SERVICE_REPLY四种
ReplyType
代表回复类型，包括INVALID，SUCCESS和FAILURE三种

有了前面的铺垫，我们开始分析SimpleMessage的实现。
首先看以下它的private数据，包括：message_type_，comm_type_，reply_code_，data_。显而易见，这些私有数据成员与协议格式中定义的HEADER和BODY部分是对应的。与之对应的提供了一些set方法，用于填充消息的各个段：

//设置消息类型ID号
void setMessageType(int msgType) {this->message_type_ = msgType;};
//设置通信类型
void setCommType(int commType) {this->comm_type_ = commType;};
//设置回复码
void setReplyCode(int replyCode) {this->reply_code_ = replyCode;};
//设置数据域
void setData(industrial::byte_array::ByteArray & data);

对应的，提供了get方法获取消息的各个段：

 int getMessageType() {return this->message_type_;};
 int getCommType() {return this->comm_type_;};
 int getReplyCode() {return this->reply_code_;};
 int getDataLength() {return this->data_.getBufferSize();};

下面看以下init初始化方法：

bool init(int msgType, int commType, int replyCode, industrial::byte_array::ByteArray &data );

init方法的参数就是消息的四个段，实现的功能就是用输入的四个参数填充内部数据，并验证消息格式是否有效，从而创建完整的消息。以下消息被认为是无效的，不可封装为消息：

commType是SERVICE_REPLY，而回复码是INVALID
commType不是SERVICE_REPLY，而回复码不是INVALID（即回复码竟然有效）

bool init(int msgType, int commType, int replyCode);

init重载版本，根据参数填充消息类型，通信类型，回复码，创建的消息数据域为空

bool init(industrial::byte_array::ByteArray & msg);

init重载版本，调用这个方法的前提是参数msg中包含了消息头或消息头+数据，在内部通过调用unload来卸载msg中的字节数据，填充到SimpleMessage的各个消息段中。

void toByteArray(industrial::byte_array::ByteArray & msg);

toByteArray方法用于向ByteArray类型的msg加载数据，注意的是在加载前，会首先清空ByteArray。