ros移动底座开发思路
开发底盘Navigator Q2的ros驱动
正常情况下，底盘驱动（ROS节点：base_controller）需要完成的两项任务为：
-
ROS内的相应节点会将运动控制的数据发往话题/cmd_vel内，底盘驱动需要订阅该话题得到控制数据（由于是2D平面，所以控制信息只包括x,y方向的速度以及垂直于平面的转速，然后将此速度控制信息发往底盘（比如通过串口、蓝牙、WIFI，考虑到通信质量以及普遍性，通常使用串口通信）。底盘得到来自上层的运动控制信息后经过运动学逆向解算将速度命令转换为每个麦克纳姆的速度，从而通过电机驱动控制电机运作。
-
由于ROS下的机器人建图、导航等一系列功能强烈依赖里程计信息，即：需要通过里程计信息辅助实现自我定位，所以底盘驱动需要返回底盘的里程计信息。一是发布odom坐标系与base_footprint的tf坐标变换；二是向/odom话题发布里程计信息（nav_msgs/Odometry）。
针对这两个任务开发底盘驱动，那么根据底盘已经提供的功能可以把开发任务分为几类（从简单到复杂）：
-
第一种也是最简单的。那就是底盘是你买的，厂家已经把底盘控制器以及对应的ROS驱动包做好了（其实现在大多数都是这样的）。比如说：turtlebot、EAI以及各种机器人厂家。可以说现在很多机器人移动平台都支持ROS，这是一种趋势。那么使用的时候，直接使用对应的命令运行launch文件启动即可。
-
第二种相对工作量大一点。底盘也是买的，但是它并不支持ROS，比如本课题的Navigator Q2（其实有ROS开发包，但没开发好）。针对这类底盘，它们本身的运动控制器肯定是写好了的，开发者可以比如使用串口、蓝牙、wifi等方式直接向底盘发送命令控制底盘运动。比如，本项目中，可以直接使用UART串口通信向Navigator Q2发送16进制的速度命令，规定格式如下（遥控器的控制协议）：
那么现在的工作就是得到/cmd_vel内的标准速度控制指令，然后转换成底盘的16进制控制格式。
-
最后一种也是最复杂的一种，也是最考验技术的一种，那就是全部自己开发。在第2步的基础上，底盘的控制也要自己写，也就是当速度命令下发来后，底盘要根据该命令转换为每个车轮的转速，中间就涉及到了小车的你运动学建模了。这项工作其实也可以分为两个方向：一是开发成第2类的小车方式（也是最主流的），意思就是小车的运动控制（逆运动学建模、控制电机等）是在底盘内完成的，然后底盘给出一个控制的接口，就成了第二类的小车，这样的好处就是便于集成，你如果想集成到ROS，那么就写一个ROS的驱动下发命令，你如果向集成到其它系统，那么就按照其它系统的规范写一个驱动，这样模块相对独立，降低耦合。二是一种耦合比较高的方式，具体就是：全部工作都在ROS下完成了，拿到速度命令后就在ROS驱动内完成逆运动学解算为各个电机转速并控制电机运动，这样做是不太推荐的，耦合高，如果到时候我不想在ROS下跑了，那么底盘的控制代码我就要全部重写。
工作空间建立
在catkin_ws/src下新建文件夹navigator_q2。
然后开始新建功能包，这里参照了比如turtlebot的规范。分别建立了两个包：q2_bringup、q2_nav。
q2_bringup放的是底盘驱动相关的文件。q2_nav则是导航相关的:gmapping、amcl、move_base相关的配置和launch文件。
程序结构
crc校验
crc校验的目的是为了验证传输数据的正确性，会把需要传输的数据按照一定的规定进行演算，得出一个校验码。所以数据接收方收到数据后，可使用相同的校验规则对数据进行校验得出校验码，然后与发送的数据包内的校验码对比。
Navigator q2用的crc校验规则叫做"CRC-16/XMODEM"，在线crc校验链接：http://www.ip33.com/crc.html
from crcmod import *
#封装成函数
def crc_exchange(data_input):
#data = data.replace(" ", "")
#print data
#转成hex
data_hex = data_input.decode("hex")
#构造crc校验器
crc16 = crcmod.mkCrcFun(0x11021, rev=False, initCrc=0x0000, xorOut=0x0000)
#生成校验值
crc = hex(crc16(data_hex))#string
#print crc
#排除高位为0而被省略的问题
if len(crc)==4:
crc='0x00'+crc[-2:]
elif len(crc)==5:
crc='0x0'+crc[-3:]
elif len(crc)==3:
crc='0x000'+crc[-1:]
#取出高、低位
print crc[2:4],crc[-2:]
#拼接完整字符串
data_output=data_input+crc[-2:].upper()+crc[2:4].upper()+'0D'
print data_output
return data_output
python串口通信
这个就比较简单了，但还是记一下吧
import serial
import time
import rospy
rospy.init_node('serial_test')
# 定义串口
ser=serial.Serial("/dev/ttyUSB2",115200,timeout=0.5)
list='AA0F012F1700003200FEFF0000000005000000000000001100001C00BE700D'
hexer=list.decode("hex")
r = rospy.Rate(10)
i=0
while True:
try:
# 写入数据
ser.write(hexer)
i=i+1
print i
except Exception as e:
print("---异常---：",e)
r.sleep()
线程编程与程序框架
总程序见附件Navigator_controller.py。
注：向底盘写入数据的部分就依靠一个订阅就好，然后在回调函数内将命令通过串口发往底盘（这里向底盘发送命令的频率就取决于/cmd_vel话题的频率）。里程反馈部分则另写一个线程实现，一个死循环，一直读串口数据（这个读数据是阻塞的，有数据才向下执行），然后按照一定的方式反馈里程（这里向ROS反馈的频率就取决于底盘向串口反馈里程的频率）。
不反馈odom的底盘驱动
那么ros底盘驱动只负责：将/cmd_vel的速度信息按对应的格式发往底盘即可。那么就等订阅/cmd_vel，等有消息了就触发回调函数。
#见附件navigator_controller_without_odom.py
def crc_exchange(data_input):
#crc校验，输出完整的的１６进制速度命令
return data_output
def velCallback(msg):
#将/cmd_vel速度命令转化为对应16进制
#crc校验
data_output=crc_exchange(data)
hexer=data_output.decode("hex")
#写入串口
ser.write(hexer)
ser=serial.Serial("/dev/ttyUSB0",115200,timeout=0.5)
rospy.init_node('teleop_navigator')
rospy.Subscriber('/cmd_vel',Twist,velCallback)
rospy.spin()
使用laser_scan_matcher发布odom到base_footprint的tf变换
附官网地址：laser_scan_matcher ROS wiki
中间参考了两篇博客：ROS AMCL定位个人问题汇总、ROS：激光雷达+laser_scan_matcher 运行gmapping
我的理解：它其实做的就是扫描匹配，利用激光雷达读入数据的变化，推算自身在环境中的位置。所以，这个过程中，需要环境有较明显的特征（机器人只要发生了移动，激光雷达的信息就会发生变化），比如在长直的走廊内，机器人如果沿着平行墙面的方向移动，激光雷达读入的数据是不会改变的（不难理解，思考一下），所以此时机器人无法做扫描匹配，不知道自己在哪。
**需要注意的是：**这个节点只是发布了odom到base_footprint的tf变换，并没有向/odom话题发布里程计信息Odometry。因为它只是利用激光雷达信息做了位姿的匹配（激光雷达信息中无法体现速度信息）。所以，严格来说，这个节点并不是发布了里程计信息，因为Odometry包含了两部分：一个是位姿（位置和姿态），一个是速度（线速度和角速度）。
安装过程：
sudo apt-get install ros-kinetic-scan-tools
然后写了个启动的launch：
<launch>
<param name="/use_sim_time" value="false"/>
<!-- 发布激光和底座的静态tf变换 -->
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="0.25 0.0 0.175 0.0 0.0 0.0 /base_footprint /laser_frame 10" />
<!-- 启动节点并设定参数 -->
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
name="laser_scan_matcher_node" output="screen">
<param name="use_imu" value="false"/>
<param name="use_odom" value="false"/>
<param name="use_cloud_input" value="false"/>
<param name="fixed_frame" value = "odom"/>
<param name="base_frame" value = "base_footprint"/>
<param name="max_iterations" value="10"/>
</node>
</launch>
问题：该节点能否用于导航过程呢
问题描述：前面也说了，正常情况下，底盘的反馈包括两个部分，tf变换和/odom话题发数据。那么laser_scan_matcher实现了tf的变换，但是没有做向/odom发送历程信息这一部分，它能否用于导航过程呢？
那么，抱着这个疑问，我就想知道：在导航过程中，到底谁在利用/odom话题呢？然后，我跑了一个假假机器人的导航实验。
roslaunch bringup simulate-amada.launch
# 打开rqt_graph，效果如下
rqt_graph
可以看到，就只有/move_base节点在订阅/odom，所以我立刻到ros wiki上去看了一下，它用/odom干嘛。
然后就发现是local_planner在用，所以又顺势打开了base_local_planner，最终发现确实是它订阅了这个话题。
我的理解是它拿到里程计实时反馈的速度就能够更好的根据反馈速度进行速度调节。
那么问题来了：那我如果就真不要/odom了，可以吗？
进行实操：
最终发现，是可以的！
gmapping
使用gmapping进行建图需要的组件。启动底盘、启动激光、启动gmapping、启动rviz。
gmapping就是使用的ros自带的gmapping包，启动gmapping一般使用launch方法，常规的launch文件如下：（一些参数默认使用即可，关于坐标系名称之类的可以修改一下）
<launch>
<arg name="scan_topic" default="scan" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value="base_footprint"/>
<param name="odom_frame" value="odom"/>
<param name="map_update_interval" value="0.5"/>
<param name="maxUrange" value="5.6"/>
<param name="maxRange" value="5.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="0.5"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="5.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.001"/>
<param name="srt" value="0.002"/>
<param name="str" value="0.001"/>
<param name="stt" value="0.002"/>
<param name="linearUpdate" value="0.2"/>
<param name="angularUpdate" value="0.1"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>
<!--
<param name="xmin" value="-50.0"/>
<param name="ymin" value="-50.0"/>
<param name="xmax" value="50.0"/>
<param name="ymax" value="50.0"/>
make the starting size small for the benefit of the Android client's memory...
-->
<param name="xmin" value="-5.0"/>
<param name="ymin" value="-5.0"/>
<param name="xmax" value="5.0"/>
<param name="ymax" value="5.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
move_base
move_base用于导航过程，在指定终点后move_base会一直向/cmd_vel输送速度命令，直至到达目标。如下是一个move_base.launch的文件：
<launch>
<arg name="odom_topic" default="odom" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find bringup)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find bringup)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find bringup)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find bringup)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find bringup)/param/base_local_planner_params.yaml" command="load" />
<rosparam file="$(find bringup)/param/move_base_params.yaml" command="load" />
<remap from="odom" to="$(arg odom_topic)"/>
</node>
</launch>
它主要注入了几个yaml文件，这几个yaml文件里面的参数关系到了各个规划器和move_base的行为。