系列文章目录
第五节 实现STM32与ubuntu系统下的ROS串口DMA通信，传输底盘速度等信息
前言
今天我们收到底盘的速度与角度信息后进行odom里程计发送
一、发布里程计
发布里程计之前我们看看里程计需要计算哪些数据
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//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.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
//publish the message
odom_pub.publish(odom);这是一个简单的发布例子，可以看出需要机器人的位置信息与速度信息，我们知道的只有速度，由数学可知位置当然可以积分出来了，这个计算方法每种底盘模型都不一样，我这里还是介绍两轮差速模型
?由数学推导可知当前位置与上一次位置的关系是
?d是小车中心的唯一，则就是两次航向角的夹角,D_L是左轮位移，D_R是右轮位移
1.位移信息计算
?则综上可得到如下计算
//中心位移
double center_d = (left_speed*time + right_speed*time)/2;
position_x = position_x_last + center_d *cos(theta) ;
position_y = position_y_last + center_d *sin(theta) ;对于odom.pose.pose.orientation，也就是方向四元数，我们得到底盘传来的yaw后调用函数
geometry_msgs::Quaternion odom_quat;
odom_quat = tf::createQuaternionMsgFromYaw(yaw);则会自动将yaw转化为四元数，其它方向角度可暂时不管
2.速度信息
为角度差，t为两次时间差
3.发布里程计
我们这里将里程计的坐标系定为odom，车体底盘定为base_footprint
// 发布里程计消息
ros::Publisher pub;
//发布odometry消息，消息队列为1
pub = n.advertise<nav_msgs::Odometry>("odom", 1);
nav_msgs::Odometry msgl;
msgl.header.stamp = time;
msgl.header.frame_id = "odom";
msgl.pose.pose.position.x = position_x;
msgl.pose.pose.position.y = position_y;
msgl.pose.pose.position.z = 0.0;
msgl.pose.pose.orientation = odom_quat;
msgl.child_frame_id = "base_footprint";
msgl.twist.twist.linear.x = linear;
msgl.twist.twist.linear.y = 0;
msgl.twist.twist.angular.z = angluar;
pub.publish(msgl);二、发布tf变换
1.内容计算
与里程计相同
2.发布tf
ros::Time time = ros::Time::now();
// 发布TF
tf::TransformBroadcaster odom_broadcaster;
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
geometry_msgs::Quaternion odom_quat;
odom_quat = tf::createQuaternionMsgFromYaw(yaw);
odom_trans.transform.translation.x = position_x;
odom_trans.transform.translation.y = position_y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
odom_broadcaster.sendTransform(odom_trans);三.rviz查看信息与launch集成节点
在CMakelist.txt内生成总的执行文件，两个文件一个串口函数定义，一个负责接受地盘数据并且发布里程计与坐标转换
add_executable(robot_start
src/publish_node.cpp
src/mbot_linux_serial.cpp )
target_link_libraries(robot_start ${catkin_LIBRARIES})机器人启动文件，并打开rviz
<launch>
<!-- 启动rebot，这里节点名写你自己发布里程计并接受串口数据的节点名称 -->
<node pkg="my_robot" type="robot_start" name="robot_start" output="screen" />
<node pkg="rviz" type="rviz" name="rviz" />
</launch>终端内
catkin_make
source ./devel/setup.bash
roslaunch my_robot robot_start #填写你的launch文件此时rviz内左下角添加tf组件，手动转动车体，查看yaw轴方向是否准备，拨动车轮，看odom坐标系与base_footprint的平移是否正常
总结
这时你可以连接好底盘与上位机，并发送数据，上位机打开rviz查看