在移動機器人建圖和導航過程中，提供相對准確的里程計信息非常關鍵，是后續很多工作的基礎，因此需要對其進行測試保證沒有嚴重的錯誤或偏差。實際中最可能發生錯誤的地方在於機器人運動學公式有誤，或者正負號不對，或者定義的坐標系之間方向不一致等。
整個移動機器人的控制結構如下圖所示，其中base_controller節點將訂閱的cmd_vel信息通過串口或其它通信接口發送給下位機（嵌入式控制板）。下位機中根據機器人運動學公式進行解算，將機器人速度轉換為每個輪子的速度，然后通過CAN總線（或其它總線接口）將每個輪子的轉速發送給電機驅動板控制電機轉動。電機驅動板對電機轉速進行閉環控制（PID控制），並統計單位時間內接收到的編碼器脈沖數，計算出輪子轉速。
base_controller節點將接收到的cmd_vel速度信息轉換為自定義的結構體或union類型的數據（自定義的數據類型中可以包含校驗碼等其它信息），並通過串口發送控制速度信息（speed_buf）或讀取機器人傳回的速度信息 (speed_buf_rev)。base_controller節點正確讀取到底層（比如嵌入式控制板）傳回的速度后進行積分，計算出機器人的估計位置和姿態，並將里程計信息和tf變換發布出去。下面的代碼只是一個例子，需要完善自定義的數據結構和校驗函數：
#include <iostream> // C++標准庫頭文件 #include <iomanip> #include <math.h> #include <boost/asio.hpp> // boost庫頭文件 #include <boost/bind.hpp> #include <ros/ros.h> // ros頭文件 #include <std_msgs/String.h> #include <nav_msgs/Odometry.h> #include <geometry_msgs/Quaternion.h> #include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PolygonStamped.h> #include <geometry_msgs/Polygon.h> #include <geometry_msgs/Point32.h> #include <tf/tf.h> #include <tf/transform_broadcaster.h> using namespace std; using namespace boost::asio; /********************************回調函數***************************************/ void cmd_velCallback(const geometry_msgs::Twist &twist_aux) { // 在回調函數中將接收到的cmd_vel速度消息轉換為自定義的結構體（或者union）類型的數據 speed_buf.vx = twist_aux.linear.x; speed_buf.vy = twist_aux.linear.y; speed_buf.vth = twist_aux.angular.z; } double x = 0.0; // 初始位置x的坐標 double y = 0.0; // 初始位置y的坐標 double th = 0.0; // 初始位置的角度 double vx = 0.0; // x方向的初始速度 double vy = 0.0; // y方向的初始速度 double vth = 0.0; // 初始角速度 double dt = 0.0; // 積分時間 int main(int argc, char** argv) { /****************************** 配置串口 ***********************************/ io_service iosev; serial_port sp(iosev, "/dev/ttyUSB0"); // 設置串口名稱 sp.set_option(serial_port::baud_rate(115200)); // 設置波特率 sp.set_option(serial_port::flow_control(serial_port::flow_control::none)); // 設置控制方式 sp.set_option(serial_port::parity(serial_port::parity::none)); // 設置奇偶校驗 sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one)); // 設置停止位 sp.set_option(serial_port::character_size(8)); // 設置字母位數為8位 ros::init(argc, argv, "base_controller"); // 初始化node節點 ros::NodeHandle n; ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 100, cmd_velCallback); // 訂閱cmd_vel topic ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10); // 發布odom topic tf::TransformBroadcaster odom_broadcaster; // 發布base_link --> odom的tf變換 ros::Publisher poly_pub = n.advertise<geometry_msgs::PolygonStamped>("polygon",10); // 發布PolygonStamped信息，rviz中顯示機器人邊界 ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); while(ros::ok()) { current_time = ros::Time::now(); read(sp, buffer(speed_buf_rev)); // 從串口讀取機器人速度數據 if(CRC_verify(speed_buf_rev)) // 對接收到的數據進行校驗 { vx = speed_buf_rev.vx; vy = speed_buf_rev.vy; vth = speed_buf_rev.vth; // 打印接收到的機器人速度信息 ROS_INFO("vx is %2f", vx); ROS_INFO("vy is %2f", vy); ROS_INFO("vth is %2f", vth); /**compute odometry in a typical way given the velocities of the robot**/ double dt = (current_time - last_time).toSec(); double delta_x = (vx * cos(th) - vy * sin(th)) * dt; double delta_y = (vx * sin(th) + vy * cos(th)) * dt; double delta_th = vth * dt; x += delta_x; y += delta_y; th += delta_th; /***********first, we'll publish the transform over tf*************/ geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = current_time; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link"; odom_trans.transform.translation.x = x; odom_trans.transform.translation.y = y; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th); // send the transform odom_broadcaster.sendTransform(odom_trans); /*********next, we'll publish the odometry message over ROS*******/ nav_msgs::Odometry odom; odom.header.stamp = current_time; odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; // since all odometry is 6DOF we'll need a quaternion created from yaw geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th); //set the position odom.pose.pose.position.x = x; odom.pose.pose.position.y = y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; // set the velocity odom.twist.twist.linear.x = vx; odom.twist.twist.linear.y = vy; odom.twist.twist.angular.z = vth; odom_pub.publish(odom); /*******************publish polygon message***********************/ geometry_msgs::Point32 point[4]; // coordinates described in base_link frame point[0].x = -0.39; point[0].y = -0.31; point[1].x = 0.39; point[1].y = -0.31; point[2].x = 0.39; point[2].y = 0.31; point[3].x = -0.39; point[3].y = 0.31; geometry_msgs::PolygonStamped poly; poly.header.stamp = current_time; poly.header.frame_id = "base_link"; // 多邊形相對於base_link來描述 poly.polygon.points.push_back(point[0]); poly.polygon.points.push_back(point[1]); poly.polygon.points.push_back(point[2]); poly.polygon.points.push_back(point[3]); poly_pub.publish(poly); } else ROS_INFO("Serial port communication failed!"); write(sp, buffer(speed_buf, buffer_length)); // 速度控制信息寫入串口 last_time = current_time; ros::spinOnce(); } // end-while iosev.run(); }
teleop_joy節點訂閱手柄發布的joy消息，並將該消息轉換為機器人速度：
#include <ros/ros.h> #include <geometry_msgs/Twist.h> #include <sensor_msgs/Joy.h> class Teleop { public: Teleop(); private: void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); ros::NodeHandle nh_; int linear_, angular_,transverse_; // 手柄上的軸號（分別表示用哪個軸控制前后移動、旋轉以及橫向運動） double l_scale_, a_scale_, t_scale_; // 速度比例系數 ros::Publisher vel_pub_; ros::Subscriber joy_sub_; }; Teleop::Teleop():linear_(1),angular_(0),transverse_(2) { // param()函數從參數服務器取參數值給變量。如果無法獲取，則將默認值賦給變量 nh_.param("axis_linear", linear_, linear_); nh_.param("axis_angular", angular_, angular_); nh_.param("axis_transverse", transverse_, transverse_); nh_.param("scale_angular", a_scale_, a_scale_); nh_.param("scale_linear", l_scale_, l_scale_); nh_.param("scale_transverse", t_scale_, t_scale_); vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1); joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Teleop::joyCallback, this); } void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) { geometry_msgs::Twist twist; // 發布的機器人速度等於joy數據乘以速度比例系數 // 比如手柄X軸向前推到最大時為1.0,速度比例系數為0.4,則機器人最大前進速度為0.4m/s twist.linear.x = l_scale_*joy->axes[linear_]; twist.linear.y = t_scale_*joy->axes[transverse_]; twist.angular.z = a_scale_*joy->axes[angular_]; vel_pub_.publish(twist); // 發布機器人速度信息 } int main(int argc, char** argv) { ros::init(argc, argv, "teleop_joy"); Teleop teleop_turtle; ros::spin(); }
為了方便測試，將相關節點寫到teleop_control.launch文件中，啟動后分別打開base_controller、joy、teleop_joy、rviz這四個節點。注意之前teleop_joy程序中用param()函數獲取參數並賦值給程序中的變量，這樣就可以將配置參數寫在launch文件中，想要改變程序中某些變量的值時直接修改launch文件就行，不用再編譯一遍源程序，調試和使用時會很方便。
<launch> <!-- Start the base_controller node --> <node pkg="slam" type="base_controller" name="base_controller" /> <!-- Start IMU message publish node --> <!-- <node pkg="imu" type="imu" name="imu" /> --> <!--Import robot_pose_ekf file into the current file --> <!-- <include file="$(find slam)/launch/robot_pose_ekf.launch" /> --> <!-- Start joy node: publish joystick message --> <node pkg="joy" type="joy_node" name="turtle_joy" respawn="true" output="screen"> <param name="dev" type="string" value="/dev/input/js0" /> <!-- Defult device name --> <param name="deadzone" value="0.12" /> </node> <!-- Axes configuration--> <param name="axis_linear" value="1" type="int" /> <!-- Axes for forward and backword movement --> <param name="axis_angular" value="0" type="int" /> <!-- Axes for counterclockwise and clockwise rotation --> <param name="axis_transverse" value="2" type="int" /> <!-- Axes for left and right movement--> <param name="scale_linear" value="0.4" type="double" /> <!-- maximum vx is 0.4m/s --> <param name="scale_angular" value="-0.3" type="double" /> <!-- maximum angular velocity is 0.3rad/s --> <param name="scale_transverse" value="0.3" type="double" /> <!-- maximum vy is 0.3m/s --> <!-- Start teleop_joy node to control the robot by joystick--> <node pkg="slam" type="teleop_joy" name="teleop_joy" /> <!-- visualization --> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam)/rviz/teleop.rviz" /> </launch>
實際測試時，機器人的移動距離和轉動角度都要進行測試（要確保機器人的實際運動方向與發送的速度指令方向一致，並且偏差在正常范圍內），如果測試與預期情況不一樣則需要查找原因。另外由於輪子打滑、以及各種誤差的影響，對速度積分進行航跡推算得到的里程計累積誤差會越來越大。實際測試時rviz中的Odometry信息（紅色箭頭）以及機器人邊界（藍色矩形）如下圖所示。機器人從一個固定參考位置開始運動，主要是前進后退以及橫向移動，最終回到起始位置。可以發現誤差還算比較小：
下面用手柄控制機器人走了更遠的距離，並且運動過程中旋轉較多（出現了打滑的情況），最終回到初始位置時Odometry的位置和角度偏差較大：
單獨使用里程計來估計小車位置姿態的效果不是特別好，因為輪子打滑等情況在實際中很難避免。因此可以考慮使用IMU或其它傳感器來同時進行測量，使用robot_pose_ekf（擴展卡爾曼濾波）對imu和里程計的數據進行融合，可以得到更准確的機器人位姿信息。注意在使用robot_pose_ekf時base_controller程序中就不應再發送base_link和odom之間的tf變換了，因為robot_pose_ekf會發布相應的變換。
下圖就是實際測試的情況，其中黃色箭頭為多傳感器信息融合后得到的里程計信息（odom_combined），紅色為單獨使用編碼器計算的里程計信息（odom）。機器人從一個固定參考位置出發，轉一圈之后回到起始位置，融合后的位姿信息更准一點。
在測試時，某些情況下需要給機器人發送一個恆定的速度，用手柄比較難做到，可以在命令行中使用rostopic pub向指定的話題上發布數據：
rostopic pub <topic-name> <topic-type> [data...]
使用rostopic pub發布消息時有3種模式：
1. 鎖存模式Latching mode：rostopic will publish a message to /topic_name and keep it latched -- any new subscribers that come online after you start rostopic will hear this message. You can stop this at any time by pressing ctrl-C.
2. 單次模式Once mode：If you don't want to have to stop rostopic with ctrl-C, you can publish in once mode. rostopic will keep the message latched for 3 seconds, then quit.
3. 循環模式Rate mode：In rate mode, rostopic will publish your message at a specific rate. For example, -r 10 will publish at 10hz. For file and piped input, this defaults to 10hz.
三種模式在命令行中對應的選項為：
1. -l, --latch：Enable latch mode. Latching mode is the default when using command-line arguments.
2. -r RATE：Enable rate mode. Rate mode is the default (10hz) when using piped or file input.
3. -1, --once：Enable once mode.
比如讓機器人以0.5m/s的速度前進，可以輸入下面的指令（如果需要循環發送控制指令機器人才能運動，可以將命令中的-1改為-r 10，即每秒發送10次速度指令）：
$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}'
參考：