在移动机器人建图和导航过程中，提供相对准确的里程计信息非常关键，是后续很多工作的基础，因此需要对其进行测试保证没有严重的错误或偏差。实际中最可能发生错误的地方在于机器人运动学公式有误，或者正负号不对，或者定义的坐标系之间方向不一致等。
整个移动机器人的控制结构如下图所示，其中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();
}
View Code
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();
}
View Code
为了方便测试，将相关节点写到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：rostopicwill publish a message to/topic_nameand keep itlatched-- any new subscribers that come online after you startrostopicwill hear this message. You can stop this at any time by pressingctrl-C.
2. 单次模式Once mode：If you don't want to have to stop rostopic withctrl-C, you can publish inonce mode.rostopicwill keep the message latched for 3 seconds, then quit.
3. 循环模式Rate mode：In rate mode,rostopicwill publish your message at a specific rate. For example,-r10will publish at 10hz. For file and piped input, this defaults to 10hz.
三种模式在命令行中对应的选项为：
1. -l,--latch：Enable latch mode. Latching mode is thedefaultwhen using command-line arguments.
2. -rRATE：Enablerate mode. Rate mode is thedefault(10hz) when using piped or file input.
3. -1,--once：Enableonce 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}}'
参考：
rostopic command-line tool
微软Xbox One无线手柄控制机器人
使用robot_pose_ekf对传感器信息融合
Publishing Odometry Information over ROS
Dashgo底盘入门教程-校准-不带陀螺仪的精度校准
ROS在rviz中实时显示轨迹（nav_msgs/Path消息的使用）
