上一篇博客我们介绍了ROS试验车的结构设计（三维模型）和部分通讯设计。上篇博客中通讯设计主要介绍了使用ROS系统进行12V10Ah和12V20Ah锂电池的通讯。 我们的目标是设计一套完整的的ROS试验车。有想法当然就要去实现它，耳边一直回响古月居课堂的名言“怕什么真理无穷，进一寸有一寸的欢喜”。 所以 JUST DO IT。下面左图为我们试验车的三维模型设计，右图为试验车实物图。
本篇博客打算介绍一下试验车上搭载的其他传感器，包括以下几个方面。
1）IMU惯性导航数据解析。 2）制作上位机进行试验车运动调试。 3）使用ROS实现整车调试（获取试验车搭载的传感器数据）。
我们采用ROS作为平台，TX2作为主控板，通过传感器的通讯协议进行数据解析，然后编写launch文件，将所有传感器全部调用起来。下面就让我们把这些问题逐个解决吧~
1、IMU惯性导航数据解析。
采用ROS作为平台，首先创建ROS工作环境，具体操作请参见上篇博客。 部署好环境之后，我们就正式开始对IMU惯性导航（以下简称惯导）数据进行解析。 进入上述创建的工作环境，在src文件夹下，创建wt_imu_publisher.py文件，将下面代码复制进文件中
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
该例程将发布/wt_imu_topic话题，自定义消息类型wt_imu_topic::wt_imu_message
"""
import os
import sys
import numpy as np
if hasattr(sys, 'frozen'):
os.environ['PATH'] = sys._MEIPASS + ";" + os.environ['PATH']
import time
import threading
import serial
from inverse_calculation import sign_10_2_16
import rospy
from car_207.msg import wt_imu_message
data_0 = []
data_1 = []
class CAR_CONTROL_TO_ROS():
def __init__(self):
# ROS节点初始化
rospy.init_node('wtzn_imu_node', anonymous=True)
# 打开imu惯导串口
self.wtzn_imu_open()
# 接收IMU惯导发来的数据，使用msg消息方式
read_imu_msg_thread = threading.Thread(target=self.read_imu_data) # type: Thread
read_imu_msg_thread.start()
# 创建一个Publisher，发布名为/power12v20ah的topic，消息类型为car_207::power12v_20ah，队列长度10
self.wt_imu_info_pub = rospy.Publisher('/wt_imu_topic', wt_imu_message, queue_size=10)
#设置循环的频率
rate = rospy.Rate(50)
self.idx = 0
while not rospy.is_shutdown():
self.idx += 1
# # 按照循环频率延时
rate.sleep()
# 接收IMU数据
def read_imu_data(self):
global roll, pitch, yaw, tt
global data_0,data_1
while 1:
try:
# time.sleep(0.01)
data_0 = self.imu_ser_.read(1)
if data_0[0] == 0x55 :
data_1 = self.imu_ser_.read(1)
if data_1[0] == 0x53:
# print("data_0[0], data_1[0]", data_0[0], data_1[0])
data_rcv = self.imu_ser_.read(9)
check_sum = data_rcv[0] + data_rcv[1] + data_rcv[2] + data_rcv[3] + data_rcv[4] + data_rcv[5] + data_rcv[6] + data_rcv[7] + data_0[0] + data_1[0]
check_sum = check_sum & 0xff
# print(data_rcv[8], check_sum)
try:
if check_sum == data_rcv[8]:
# print("check_sum, data_rcv[8]", check_sum, data_rcv[8])
roll = ((data_rcv[1] << 8) | data_rcv[0]) / 32768 * 180
pitch = (data_rcv[3] << 8 | data_rcv[2]) / 32768 * 180
yaw = (data_rcv[5] << 8 | data_rcv[4]) / 32768 * 180
tt = (data_rcv[7] << 8 | data_rcv[6]) / 100
#
wt_imu_msgs = wt_imu_message()
wt_imu_msgs.name = "imu"
wt_imu_msgs.imu_roll = roll
wt_imu_msgs.imu_pitch = pitch
wt_imu_msgs.imu_yaw = yaw
# print(wt_imu_msgs.name, wt_imu_msgs.imu_roll, ywt_imu_msgs.imu_pitchaw, tt)
# 发布消息
self.wt_imu_info_pub.publish(wt_imu_msgs)
rospy.loginfo("Publsh power12v20ah message[%s, %.3f, %.3f, %.3f]",
wt_imu_msgs.name, wt_imu_msgs.imu_roll,
wt_imu_msgs.imu_pitch, wt_imu_msgs.imu_yaw)
except:
pass
except:
pass
# 打开串口
def wtzn_imu_open(self):
try:
# print("read port")
self.imu_ser_ = serial.Serial('/dev/ttyUSB3', 9600, timeout=0.15) # com_kou
print("imu_ser_", imu_ser_)
self.recv() # 将数据进行解析
except:
try:
if self.imu_ser_.isOpen():
print('imu_ser_ 串口已打开')
else:
print('imu_ser_ 串口未打开')
except:
print('imu_ser_ 检测不到串口')
# 关闭串口
def imu_ser_close(self):
try:
self.imu_ser_.close()
except:
print('imu_ser_ 串口已关闭')
# 全部退出程序
def quit(self):
self.imu_ser_.close()
os._exit(0)
if __name__ == '__main__':
try:
car_control_full = CAR_CONTROL_TO_ROS()
except rospy.ROSInterruptException:
pass
msg/wt_imu_message.msg中定义如下：
string name
float32 imu_roll
float32 imu_pitch
float32 imu_yaw
接下来返回到~/catkin_ws文件夹下
cd..
然后在terminal1中运行
roscore
在terminal2中运行
source devel/setup.bash
rosrun wt_imu_publisher.py
现在应该可以看到ROS一直发布惯导的姿态角度等信息。购买惯导传感器一般都会附带上位机程序，我们可以将自己解析惯导数据与上位机读取的数据进行对比，从而验证我们解析数据的正确性。
2、制作上位机进行试验车运动调试。
我们现在这款试验车的底盘的选型已经在上篇博客介绍的很详细了，试验车目前控制还是用的遥控器，虽然要控制比较方便省事。但是，也存在一定的缺点。比如：
1）四个电机时而不同步，造成试验车不按规定的路线行走（可以微调）。
2）试验车原地转圈会产生共振，造成车辆整体震动，并且震动幅度较大（也有可能是试验车上面我们设计的机构不合理造成的），后面需要进行优化。
3）试验车承重能力较弱（5.5Kg），容易造成超重。 当然，首先需要说明的是我们选用的这款试验车底盘的性价比是很高的，后面随着实验的不断推进，虽然会暴露一些问题，但是也都是可以进行优化改进的。 我们使用PYQT制作了一个简单的上位机程序，通过USB转RS232数据线与试验车底盘驱动板建立通讯，使用Python库中Serial模块进行串口通讯。（该试验在windows下进行，如果在linux下调试需要更改串口名称） 下图为试验车底盘进行调试的过程，上位机程序简单，主要实现试验车的前进、后退、左转、右转这几个动作。
由于选购试验车是带有12V10Ah的锂电池的，根据上篇博客介绍的电池报文协议解析，我们摘取主要部分放到上位机程序，能够通过上位机实时读取电池的剩余电量以及电压数据。
3）使用ROS实现整车调试（获取试验车搭载的传感器数据）。
最终实现的效果如下图所示。试验车搭载velodyne三维激光雷达、惯导、路由器、TX2开发板、双目相机和两块12V锂电池，上位机监控界面采用的是ROS的rviz、rqt_plot进行显示。(可应用rviz、rqt_plot、rqt_graph等工具，当然后面也可以考虑使用PYQT和ROS集合的方式…) 上位机显示包括：三维点云数据显示、三维点云深度图显示、相机二维图像显示、惯导数据显示、锂电池电压电量显示等。
下图为windows下开发的velodyne和IMU程序界面。前段时间自己做过一些小程序，打算在windows下制作一款上位机进行雷达数据采集、处理等。但是，由于时间安排问题，导致一直没有进展。过段时间一定好好整理一下。
细心的小伙伴可能会发现我们的这款试验车上面没有搭载显示器，也就是说在试验车运动的时候是没有办法进行数据的监控的。这个问题主要是选用的试验车的承载能力有限，上面也有提到试验车最大承载5.5kg。为了解决这个问题，我们可以考虑以下两个方案： 1）更换试验车底盘，对试验车底盘进行重新优化设计，提高最大承载能力，加装显示器。 2）采用远程控制的方式。
