ROS试验车设计②

317
0
2020年9月11日 10时31分

上一篇博客我们介绍了ROS试验车的结构设计(三维模型)和部分通讯设计。上篇博客中通讯设计主要介绍了使用ROS系统进行12V10Ah和12V20Ah锂电池的通讯。

我们的目标是设计一套完整的的ROS试验车。有想法当然就要去实现它,耳边一直回响古月居课堂的名言“怕什么真理无穷,进一寸有一寸的欢喜”。 所以 JUST DO IT。下面左图为我们试验车的三维模型设计,右图为试验车实物图。

ROS试验车设计②插图1-1

本篇博客打算介绍一下试验车上搭载的其他传感器,包括以下几个方面。

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下调试需要更改串口名称)

下图为试验车底盘进行调试的过程,上位机程序简单,主要实现试验车的前进、后退、左转、右转这几个动作。

2-1

由于选购试验车是带有12V10Ah的锂电池的,根据上篇博客介绍的电池报文协议解析,我们摘取主要部分放到上位机程序,能够通过上位机实时读取电池的剩余电量以及电压数据。

3)使用ROS实现整车调试(获取试验车搭载的传感器数据)。

最终实现的效果如下图所示。试验车搭载velodyne三维激光雷达、惯导、路由器、TX2开发板、双目相机和两块12V锂电池,上位机监控界面采用的是ROS的rviz、rqt_plot进行显示。(可应用rviz、rqt_plot、rqt_graph等工具,当然后面也可以考虑使用PYQT和ROS集合的方式…)

上位机显示包括:三维点云数据显示、三维点云深度图显示、相机二维图像显示、惯导数据显示、锂电池电压电量显示等。

3-1

下图为windows下开发的velodyne和IMU程序界面。前段时间自己做过一些小程序,打算在windows下制作一款上位机进行雷达数据采集、处理等。但是,由于时间安排问题,导致一直没有进展。过段时间一定好好整理一下。

ROS试验车设计②插图(4)

细心的小伙伴可能会发现我们的这款试验车上面没有搭载显示器,也就是说在试验车运动的时候是没有办法进行数据的监控的。这个问题主要是选用的试验车的承载能力有限,上面也有提到试验车最大承载5.5kg。为了解决这个问题,我们可以考虑以下两个方案:

1)更换试验车底盘,对试验车底盘进行重新优化设计,提高最大承载能力,加装显示器。

2)采用远程控制的方式。

本节暂时就先介绍这么多吧。相机数据解析、远程控制的方式等问题我们后续会进行补充。

记得点赞哦!

发表评论

后才能评论