通訊過程開始:
首先是從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通訊啦....