rtab_map可以基于雙目/RGB-D相機生成稠密點雲地圖，并基于此生成可生成用于導航的機率栅格地圖。而ROS中的Navigation包中的move_base子產品內建了常用的機器人全局和局部路徑規劃算法，如A*、Dijkstra、動态視窗等。我們以RTAB_MAP和move_base作為基礎，導航生成規劃速度指令，以/cmd_vel話題的形式釋出。本文的重點在于，如何訂閱/cmd_ve話題，并提取其中的線速度和角速度資訊，釋出到輪速機器人底盤控制機器人按照規劃的軌迹運動。
一、cmd_vel話題解析
在ros中查閱/cmd_vel話題的相關資訊如下，其消息格式包括線速度和角速度。如下圖坐标系所示，二維導航輸出結果包含的有效資訊實則包括linear.x （x方向線速度）和linear.y（y方向線速度）以及angular.z（繞z軸角速度），其中線速度的機關為m/s，并且通常linear.y為0；角速度的機關為rad/s，正表示逆時針，負表示順時針。
一個實際的例子，如下圖所示。
二.、角速度和線速度轉化為左右車輪速
首先左、右輪的轉速決定了前進的速度和方向，當角速度為0時，左右輪速一樣，其值為
V_r=V_l=V_cen=(V_cur/V_max)*400
其中400為我自身幀格式定義的，0~400對應着轉速0-100%。
當左右輪速有差時，對應會産生角速度，轉向。理論上是有公式計算的，但會有偏差，是以采用實際測量拟合的方式。記錄車轉向pi/2、pi、3/2pi、2pi時經過的時間和左右輪速差， 拟合得到系數
r=((pi/2)/dev_t)*deta_pi
那麼，角速度不為0時，左右輪速值為;
V_r=V_cen+W_cur*t*r
V_l=V_cen-W_cur*t*r
其中t為cmd_vel指令更新的時間間隔，在我的算法中，用
rostopic hz /cmd_vel
指令檢視頻率後，近似取為1.25。
三、輪速指令的傳輸
訂閱ROS中的cmd_vel話題後，可通過相關轉化得到左右車輪的輪速，但是控制端隻能通過序列槽輸出，而輪速機器人一般CAN總線控制，是以還需要打通通信鍊路。
本人的現有的狀态是，建圖導航、cmd_vel話題解析這相關動作都是在ubuntu16.04的PC端完成，而機器人隻接受CAN通信協定的指令輸入。考慮到有兩種選擇，一是PC端通過CAN卡，将序列槽協定轉化為CAN協定；而是采用STM32作為中轉，利用其內建的CNA接口輸出指令帶CAN總線上。另外由于手上現有的CAN卡為單通道，為毫米波雷達所用，是以采取方案2。
接下來就是調試整個通信鍊路。
首先PC端序列槽發送包含左、右輪速資訊的消息，STM32開發序列槽1接收消息，這一步很順利的調試通過。
緊接着需要STM32接收到指令消息後通過CNA接口發送給機器人，這一步就很坑了。我們手上的開發闆沒有引出CNA接口，并且沒有CAN總線電平的發送和接收電路。觀察發現開發闆序列槽2有引出RS485的接口，糾結一番後淘寶購入RS485轉CAN的轉換頭。RS485轉CAN的轉換頭有一個好處是其支援透傳和格式轉發，不需要完全掌握CAN總線的基礎知識，即可完成can通信。利用賣家贈送的配置軟體配置can總線的波特率、發送間隔等參數後，可順利通過485端發送指令控制機器人運動。
轉接頭調試成功後，将其接到序列槽2的輸出端，然後将轉接頭CAN輸出端接到車的控制總線上。緊接着調試整個控制流程，此處遇到一個小坑，RS485是半雙工通信，不能同時收發，需要一個端口控制收發模式的切換。我的開發闆序列槽2輸出是PA2和PA3端口，控制是PB3，但是PB3同時是JTAG接口，需要禁用JTAG功能後再複用，不然無法成功切換模式。
最後，序列槽1和PC連接配接，PC發送指令，序列槽1接收到後通過序列槽2（RS485接口）發送到RS485轉CAN的轉換頭，轉換頭的CAN輸出接到機器人控制端，成功實作指令控制。