移動底座與ROS上層的通信一般採用串口或者CAN接口，本次設計選用串口與ROS上層通信。Navigation Stack 中發佈給 base_controller 的話題爲 cmd_vel ，因此需要設計一個節點用於接收 cmd_vel 話題，獲取該話題中的消息將其轉換成移動底座可識別的速度及角速度指令，通過串口發送給移動底座，從而控制移動底座按既定要求運動。該節點還需要接收底座的通過串口上傳過來的里程編碼消息並轉換成里程計消息發佈到 ROS 上層，爲 ROS 導航提供必須的里程計消息。
訂閱cmd_vel話題
1.設計一個節點，catkin_create_pkg serial_example roscoo rospy std_msgs
2.修改CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(serial_example)
find_package(catkin REQUIRED COMPONENTS
roscpp
serial
std_msgs
)
catkin_package(
CATKIN_DEPENDS
serial
std_msgs
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(serial_example_node src/serial_example_node.cpp)
target_link_libraries(serial_example_node
${catkin_LIBRARIES}
)3.package.xml
1 <?xml version="1.0"?>
2 <package>
3 <name>serial_example</name>
4 <version>0.0.0</version>
5 <description>Example package for using http://wjwwood.io/serial/ library</description>
6
7 <author email="[email protected]">Gary Servin</author>
8
9 <maintainer email="[email protected]">Gary Servin</maintainer>
10
11 <license>BSD</license>
12
13 <buildtool_depend>catkin</buildtool_depend>
14 <build_depend>serial</build_depend>
15 <build_depend>std_msgs</build_depend>
16 <run_depend>serial</run_depend>
17 <run_depend>std_msgs</run_depend>
18
19 </package>cd ~/joey_ws/src
git clone git://github.com/wjwwood/serial.git
我們暫時命名爲 my_serial_node ,在該節點中藉助 ros-serial 實現串口的收發功能，並訂閱話題
cmd_vel, 爲了方便測試我們這裏暫時訂閱 /turtle1/cmd_vel 話題，詳細看代碼：
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
|
serial::Serial ser; //訂閱turtle1/cmd_vel話題的回調函數，用於顯示速度以及角速度 void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel){ ROS_INFO("I heard linear velocity: x-[%f],y-[%f],",cmd_vel.linear.x,cmd_vel.linear.y); ROS_INFO("I heard angular velocity: [%f]",cmd_vel.angular.z); std::cout << "Twist Received" << std::endl; } int main (int argc, char** argv){ ros::init(argc, argv, "my_serial_node"); ros::NodeHandle nh; //訂閱/turtle1/cmd_vel話題用於測試 $ rosrun turtlesim turtle_teleop_key ros::Subscriber write_sub = nh.subscribe("/turtle1/cmd_vel",1000,cmd_vel_callback); ros::Publisher read_pub = nh.advertise<std_msgs::String>("sensor", 1000); try { ser.setPort("/dev/ttyUSB0"); ser.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(1000); ser.setTimeout(to); ser.open(); } catch (serial::IOException& e) { ROS_ERROR_STREAM("Unable to open port "); return -1; } if(ser.isOpen()){ ROS_INFO_STREAM("Serial Port initialized"); }else{ return -1; }
|
可以通過以下方式簡單的測試一下訂閱的話題是否可行：
|
1 2
|
roscore & rosrun turtlesim turtle_teleop_key
|
在另外一個 terminal 中運行上面的節點:
|
1
|
rosrun serial_example serial_example_node
|
當用手按下上下左右方向鍵時會看到實時的打印速度信息：
串口發送
節點訂閱了 cmd_vel 話題，將速度、角速度數據提取出來之後，需要將這些信息以固定的協議格式通過串口送至移動底座，移動底座接收該命令並執行該命令。這裏的協議格式可自行定義，也可以直接發送速度、角速度數據到移動底座；本次開發定義格式如下：
| head | head | A-speed | B-speed | C-speed | CRC |
|---|---|---|---|---|---|
| 0xff | 0xfe | float | float | float | u8 |
因此串口發送的總字節數是15字節，在獲取到cmd_vel話題後提取有效數據並打包送至底層，串口發送藉助 ros-serial 的
write 函數功能即可。
|
1
|
serial.write(buffer,buffersize)
|
串口接收與發佈 odometry 話題
設計本節點還需要通過串口從移動底座獲取里程計信息，併發布該里程計信息，爲 Navigation stack 提供必要的 odometry 信息。串口接收的數據格式定義如下：
| head | head | x-position | y-position | x-speed | y-speed | angular-speed | pose-angular | CRC |
|---|---|---|---|---|---|---|---|---|
| 0xff | 0xae | float | float | float | float | float | float | u8 |
因此串口接收數據總字節數爲27字節，串口接收完成後通過校驗分析數據無誤後，提取對應數據填充到 odometry 消息中，併發布出去。串口接收 ros-serial 提供了很多接口函數，因爲底層發送的都是16進制格式，因此數據接收我選擇了
Serial::read (uint8_t *buffer, size_t size) 函數，這個可以從 ros-serial 的源碼中查到。里程計消息的發佈可以參考站內文章: ROS初級十五
發佈里程計消息
運動學解析函數
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
|
/********************************************************** * 運動學解析函數 將獲取的 x y 方向的線速度以及角速度轉變成 * 移動底座三個輪子的速度，並通過串口發送出去 * VA = Vx + L*W * VB = -Vx*cosθ + Vy*sinθ + L*W * VC = -Vx*cosθ - Vy*sinθ + L*W * θ 爲輪子與移動底座座標系X軸的夾角，此處爲60° * VA VB VC 分別爲三個輪子的速度 * Vx Vy 爲移動底座在x y 方向的速度 * W 爲移動底座繞Z軸的角速度 * L 爲三個輪子到移動底座中心的距離 此處爲0.15m * ********************************************************/ void kinematics_analysis(const geometry_msgs::Twist& cmd_vel){ float_union VA,VB,VC; VA.fvalue = cmd_vel.linear.x + 0.15*cmd_vel.angular.z; VB.fvalue = -cmd_vel.linear.x * 0.5 + cmd_vel.linear.y * 0.867 + 0.15*cmd_vel.angular.z; VC.fvalue = -cmd_vel.linear.x * 0.5 - cmd_vel.linear.y * 0.867 + 0.15*cmd_vel.angular.z; memset(s_buffer,0,sizeof(s_buffer)); //數據打包 s_buffer[0] = 0xff; s_buffer[1] = 0xfe; //VA s_buffer[2] = VA.cvalue[0]; s_buffer[3] = VA.cvalue[1]; s_buffer[4] = VA.cvalue[2]; s_buffer[5] = VA.cvalue[3]; //VB s_buffer[6] = VB.cvalue[0]; s_buffer[7] = VB.cvalue[1]; s_buffer[8] = VB.cvalue[2]; s_buffer[9] = VB.cvalue[3]; //VC s_buffer[10] = VC.cvalue[0]; s_buffer[11] = VC.cvalue[1]; s_buffer[12] = VC.cvalue[2]; s_buffer[13] = VC.cvalue[3]; //CRC s_buffer[14] = s_buffer[2]^s_buffer[3]^s_buffer[4]^s_buffer[5]^s_buffer[6]^s_buffer[7]^ s_buffer[8]^s_buffer[9]^s_buffer[10]^s_buffer[11]^s_buffer[12]^s_buffer[13]; ser.write(s_buffer,sBUFFERSIZE); }
|
測試
在與底盤實際通信前我們先用兩臺電腦進行串口的通信測試，並查看發佈的 odometry 消息。一臺電腦跑ROS並通過串口與另一臺電腦連接，串口監控軟件送出一組模擬數據：
|
1 2 3 4
|
0xff 0xae 0x01 0x02 0x03 0x04 0x05 0x06 0x07 0x08 0x09 0x00 0x01 0x02 0x03 0x04 0x05 0x06 0x07 0x08 0x09 0x00 0x12 0x13 0x14 0x15 0x00
|
運行節點：
|
1 2
|
roscore & rosrun my_serial_node my_serial_node
|
通過串口監控軟件不停的發送上述數據幀，並通過以下命令查看話題：
|
1
|
rostopic echo /odom
|
可以看到串口接收到底層送來的數據並實時的進行發佈：
