rosserial_python serial_node.py分析

通讯过程开始:

首先是从PC端发送请求topic_id的帧:

对应的python代码是:

1 def requestTopics(self):
2         """ Determine topics to subscribe/publish. """
3         self.port.flushInput()
4         # request topic sync
5         self.port.write("\xff" + self.protocol_ver + "\x00\x00\xff\x00\x00\xff")

正如上图, 帧内容就是: 0xFF 0xFE 0x00 0x00 0xFF 0x00 0x00 0xFF

 

然后mbed就会回复:

具体帧内容是:

FF FE 08 00 F7 0A 00 00 00 00 00 00 00 00 00 F5...

查看一下对应接受的Python代码发现规则是:

                flag = [0,0]
                flag[0] = self.tryRead(1)
                if (flag[0] != '\xff'):
                    continue

                flag[1] = self.tryRead(1)
                if ( flag[1] != self.protocol_ver):
                    self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
                    rospy.logerr("Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
                    protocol_ver_msgs = {'\xff': 'Rev 0 (rosserial 0.4 and earlier)', '\xfe': 'Rev 1 (rosserial 0.5+)', '\xfd': 'Some future rosserial version'}
                    if (flag[1] in protocol_ver_msgs):
                        found_ver_msg = 'Protocol version of client is ' + protocol_ver_msgs[flag[1]]
                    else:
                        found_ver_msg = "Protocol version of client is unrecognized"
                    rospy.loginfo("%s, expected %s" % (found_ver_msg, protocol_ver_msgs[self.protocol_ver]))
                    continue

                msg_len_bytes = self.tryRead(2)
                msg_length, = struct.unpack("<h", msg_len_bytes)

                msg_len_chk = self.tryRead(1)
                msg_len_checksum = sum(map(ord, msg_len_bytes)) + ord(msg_len_chk)

                if msg_len_checksum % 256 != 255:
                    rospy.loginfo("wrong checksum for msg length, length %d" %(msg_length))
                    rospy.loginfo("chk is %d" % ord(msg_len_chk))
                    continue

                # topic id (2 bytes)
                topic_id_header = self.tryRead(2)
                topic_id, = struct.unpack("<h", topic_id_header)c

                try:
                    msg = self.tryRead(msg_length)
                except IOError:
                    self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Packet Failed : Failed to read msg data")
                    rospy.loginfo("Packet Failed :  Failed to read msg data")
                    rospy.loginfo("msg len is %d",len(msg))
                    raise

                # checksum for topic id and msg
                chk = self.tryRead(1)
                checksum = sum(map(ord, topic_id_header) ) + sum(map(ord, msg)) + ord(chk)

                if checksum % 256 == 255:
                    self.synced = True
                    try:
                        self.callbacks[topic_id](msg)
                    except KeyError:
                        rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
                    rospy.sleep(0.001)
                else:
                    rospy.loginfo("wrong checksum for topic id and msg")

哦, 后面还有一个注释:

# modified frame : header(2 bytes) + msg_len(2 bytes) + msg_len_chk(1 byte) + topic_id(2 bytes) + msg(x bytes) + msg_topic_id_chk(1 byte)
# second byte of header is protocol version

 

 第一个字节FF是帧开始, FE表示帧版本, 08 00, 这两个字节表示这个帧后面的长度, F7是一个校验码, topic_id有2个字节, 0A 00, 接着后面有8个00, 就是包的内容, 最后是一个topic_id的校验码.

校验码的算法如下:

长度校验码:

msg_len_checksum = 255 - ( ((length&255) + (length>>8))%256 )

 

消息校验码:

msg_checksum = 255 - ( ((topic&255) + (topic>>8) + sum([ord(x) for x in msg]))%256 )

 

-----------------------------全通了的分隔线--------------------------------------

上面看到的topic_id到底是个啥意思, 其实是规定帧的topic, 跟ROS里面的topic是没毛关系的.

具体内容可以看mbed的TopicInfo.h文件

https://developer.mbed.org/users/garyservin/code/ros_lib_indigo/docs/fd24f7ca9688/TopicInfo_8h_source.html

  class TopicInfo : public ros::Msg
  {
    public:
      uint16_t topic_id;
      const char* topic_name;
      const char* message_type;
      const char* md5sum;
      int32_t buffer_size;
      enum { ID_PUBLISHER = 0 };
      enum { ID_SUBSCRIBER = 1 };
      enum { ID_SERVICE_SERVER = 2 };
      enum { ID_SERVICE_CLIENT = 4 };
      enum { ID_PARAMETER_REQUEST = 6 };
      enum { ID_LOG = 7 };
      enum { ID_TIME = 10 };
      enum { ID_TX_STOP = 11 };
.....

10其实就是0x0A, 因为这个mbed没有本地时间, 所以, 内容啥也没有.

接着mbed发了一大串东西, 从逻辑分析仪导出来的csv总结如下:

第一部分是发送第一个publisher的信息:

5.816288,'255' (0xFF),,       帧开始
5.8163835,'254' (0xFE),,     帧版本
5.816479,J (0x4A),,             帧长度, 74个字节
5.8165745,'0' (0x00),,        
5.81667,'181' (0xB5),,         帧长度校验码
5.816766,'0' (0x00),,           这个00有讲究, 就是topic_id, 参考上面的C++代码的枚举定义, 00表示下面是个publisher
5.8168615,'0' (0x00),,
--------------------------------
5.816957,} (0x7D),,            0x7D=125, 计算方法参考下面的注释1
5.8170525,'0' (0x00),,
--------------------------------
5.817148,\t (0x09),,           这个字节跟下面4个字节表示接下来的长度为9
5.8172435,'0' (0x00),,
5.8173395,'0' (0x00),,
5.817435,'0' (0x00),,
--------------------------------
5.8175305,m (0x6D),,        下面是mbed_odom, 刚好9个字节, 是publisher的名字
5.817626,b (0x62),,
5.8177215,e (0x65),,
5.817817,d (0x64),,
5.8179125,_ (0x5F),,
5.8180085,o (0x6F),,
5.818104,d (0x64),,
5.8181995,o (0x6F),,
5.818295,m (0x6D),,
--------------------------------
5.8183905,'15' (0x0F),,      接着是ox0F, 定义接下是字符长度是15个字节
5.818486,'0' (0x00),,
5.818582,'0' (0x00),,
5.8186775,'0' (0x00),,
-------------------------------
5.818773,s (0x73),,            std_msgs/String, 一共15个字节
5.8188685,t (0x74),,
5.818964,d (0x64),,
5.8190595,_ (0x5F),,
5.819155,m (0x6D),,
5.819251,s (0x73),,
5.8193465,g (0x67),,
5.819442,s (0x73),,
5.8195375,/ (0x2F),,
5.819633,S (0x53),,
5.8197285,t (0x74),,
5.819824,r (0x72),,
5.81992,i (0x69),,
5.8200155,n (0x6E),,
5.820111,g (0x67),,
----------------------------------
5.8202065,' ' (0x20),,        接下来字节长度为0x20, 就是32个  
5.820302,'0' (0x00),, 
5.8203975,'0' (0x00),,
5.8204935,'0' (0x00),,
---------------------------------
5.820589,9 (0x39),,           这个接下来的魔术数字, 来自...参考注释2
5.8206845,9 (0x39),,
5.82078,2 (0x32),,
5.8208755,c (0x63),,
5.820971,e (0x65),,
5.8210665,8 (0x38),,
5.8211625,a (0x61),,
5.821258,1 (0x31),,
5.8213535,6 (0x36),,
5.821449,8 (0x38),,
5.8215445,7 (0x37),,
5.82164,c (0x63),,
5.821736,e (0x65),,
5.8218315,c (0x63),,
5.821927,8 (0x38),,
5.8220225,c (0x63),,
5.822118,8 (0x38),,
5.8222135,b (0x62),,
5.822309,d (0x64),,
5.822405,8 (0x38),,
5.8225005,8 (0x38),,
5.822596,3 (0x33),,
5.8226915,e (0x65),,
5.822787,c (0x63),,
5.8228825,7 (0x37),,
5.822978,3 (0x33),,
5.823074,c (0x63),,
5.8231695,a (0x61),,
5.823265,4 (0x34),,
5.8233605,1 (0x31),,
5.823456,d (0x64),,
5.8235515,1 (0x31),,
-----------------------------
5.8236475,'0' (0x00),,      接下来4个bytes, 其实组成但是0200, 就是200, 200的16进制数是512, 这个数也是一个固定数, 来源参考注释3    
5.823743,'2' (0x02),,
5.8238385,'0' (0x00),,
5.823934,'0' (0x00),,

------------------------------
5.8240295,f (0x66),,      最后肯定是个校验码

 

注释1:

这个是mbed上面的ros_lib_indigo的node_handle.h, 调用nh的publisher注册的时候, 会用MAX_SUBSCRIBERS的值作为一个输入, 当做publisher的一个id, 估计ROS里面的publisher id也是这么计算的.

MAX_SUBSCRIBERS=25, 所以第1个(数组里面第0个)的id就是125

      bool advertise(Publisher & p)
      {
        for(int i = 0; i < MAX_PUBLISHERS; i++){
          if(publishers[i] == 0){ // empty slot
            publishers[i] = &p;
            p.id_ = i+100+MAX_SUBSCRIBERS;
            p.nh_ = this;
            return true;
          }
        }
        return false;
      }

 

注释2:

32个byte的东西看起来像啥?肯定是MD5校验啦...

这个MD5的校验其实是在校验msg类型的, 跟topic_name无关, 这个东西在mbed的lib里面, 有关std_msgs/String.h里面有一个函数可以直接返回这个MD5结果:

const char * getType(){ return "std_msgs/String"; };
const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1";

 

注释3:

node_handler里面是这么拼的:

ti.topic_id = publishers[i]->id_;
ti.topic_name = (char *) publishers[i]->topic_;
ti.message_type = (char *) publishers[i]->msg_->getType();
ti.md5sum = (char *) publishers[i]->msg_->getMD5();
ti.buffer_size = OUTPUT_SIZE;
publish( publishers[i]->getEndpointType(), &ti );

最后这段, 是个OUTPUT_SIZE, 是个常量就等于512, 其实跟串口设置的Buffer_Size估计有点儿关系吧.

 

上面就是对串口监听并分析迷样的结果, 有了这个, 就可以方便的伪装成mbed, 用普通的STM32板子直接用串口加USB串口转接工具跟PC通讯啦....

 

转载于:https://www.cnblogs.com/Montauk/p/7158597.html