小车控制ROS驱动包开发
####写在前面####
对于做纯SLAM算法和应用功能开发的朋友来说，其实不需要搞明白小车的底层硬件原理和软件实现等繁琐的细节，只需要通过上层API调用来实现小车的控制等操作就可以了。因此，本文开始讲解用于提供上层API调用的ROS驱动包，本文主要内容：
1) 通信协议简介（stm32与ROS驱动包通信）
2) ROS接口（topic订阅与发布）
3) ROS驱动包程序解析
4) 基于航迹推演的里程计解算
####正文####
1.通信协议简介（stm32与ROS驱动包通信）
通信协议都基于串口通信，通信数据帧由串口通信的单个字节组合而成，stm32的串口2由“小车控制-ROS驱动包”驱动，stm32的串口1由“小车debug-ROS驱动包”驱动，两个串口都使用115200的波特率。下面分别介绍两个驱动包的通信协议：
stm32与小车控制ROS驱动包通信：
不难发现，其实“小车控制-ROS驱动包”发送和接收数据遵循同样的协议。
stm32与小车debug-ROS驱动包通信：
stm32发送过来的调试信息时纯字符串流，可以通过printf()直接来查看；
小车debug-ROS驱动发送给stm32的kp、ki、kd是一个扩大10000倍后的整数，除以10000后使用。
2. ROS接口（topic订阅与发布）
ROS接口为上层应用提供API调用，便于SLAM算法和应用功能开发。
订阅topic:
/cmd_vel (geometry_msgs::Twist)
发布topic:
/enc_l (msgs::Int16)
/enc_r (msgs::Int16)
/odom (nav_msgs::Odometry)
/tf (odom->base_footprint)
3.ROS驱动包程序解析
了解了前面的通信协议和ROS接口后，接下来就来看看ROS程序是如何实现的。首先程序订阅/cmd_vel作为用户的控制输入，将控制输入的速度信息转换为通信协议中规定的格式，然后通过串口下发给stm32,实现对小车的运动控制。
下图是订阅/cmd_vel的回调函数：
下图是串口下发函数：
知道了怎么控制小车运动，接下来介绍获取小车反馈回来的码盘信息，并将反馈回来的码盘信息进行里程计解算。
如下图是串口读取和里程计解算函数：
如下图是里程计和相关topic发布：
4.基于航迹推演的里程计解算
首先通过下面的一张图来了解一下里程计解算的大致原理，下图包含两个坐标分别对应图A、图B，请看图：
在图A中，设定机器人初始位置与坐标系原点重合，机器人的正前方为坐标系x轴正方向，机器人方向角定义为由坐标系x轴正方向逆时针方向(即图中theta轴所示)。利用极限的思想，机器人在很短的时间delta_t内，由A位置运动到B位置，机器人绕p转动一个微小的角度，其中delta_d_l与delta_d_r分别为左轮和右轮的位移量，可以通过编码反馈值计算得；delta_theta是方向角变化量，由等腰三角形的关系可以得出delta_theta与图中各角的关系；L是两轮之间的轴距。有了这个理论基础，下面接着介绍如何解算出里程计。
在图B中，主要说明了如何通过迭代来计算机器人在坐标系中的姿态，从而得到我们需要的里程计。通过图A中介绍的原理，不难得出delta_d、delta_theta、delta_x、delta_y各变化量，然后通过过迭代的方法便可以求出里程计中的机器人位置(x,y,theta)和机器人的速度(v,w)。
有了里程计解算的理论基础后，接下来谈谈具体程序实现，如下图：
程序的输入是在采样周期内得到的左右轮编码值(delta_encode_left和delta_encode_right)，这个编码值可以通过速度系数speed_ratio便可以换算成实际的位移量，速度系数speed_ratio可以通过后续的标定得到；两个轮子的轴距wheel_distance也是通过后续的标定得到，采样周期encode_sampling_time在程序中是一个可选的参数。有了delta_encode_left、delta_encode_right的值，便可以推算出delta_d、delta_theta，继而可以推算出delta_x、delta_y，继续往下使用迭代可以计算出最终的里程计值position_x、position_y、oriention、velocity_linear、velocity_angular。最后通过ROS将里程计发布出去就ok了。
原文地址：https://www.cnblogs.com/hiram-zhang/p/9676725.html