1、试验车的结构设计
试验车三维结构示意图如图1所示。试验车采用铝合金支架连接,因铝合金质量较轻,不仅可满足试验车最大承重要求,采用支架结构还有利于以后的试验车结构上的扩展。试验车上包含的传感器有:三维激光雷达、惯导、路由器、TX2开发板、双目相机和两块12V锂电池(一块为试验车电机专用,另一块为试验车传感器供电)。 试验采用的三维激光雷达为velodyne-16线激光雷达;惯导为维特智能JY60型号的IMU;路由器采用MiKro TIK千兆双频路由器;TX2开发板为搭载8G运行内存的NVIDIA产品。两块锂电池,直流电机用锂电池为12V10Ah锂电池;传感器供电电池为12V20Ah锂电池。
图1 试验车三维结构示意图
试验车底盘参数如图2所示。 所用试验车电机为四驱直流电机,电机驱动芯片采用STM32作为驱动器,电机控制方式有两种:1、遥控器直接控制(比较简单,暂时略过)。2、使用上位机控制STM32驱动器,进而控制电机。
图2 试验车底盘参数
试验车电机驱动器和遥控器如图2所示。电机驱动器支持多接口扩展,例如可使用CAN通讯、RS232、485等通讯方式。本次试验采用USB转RS232控制方式,由上位机通过USB转232接线盒与电机驱动器建立连接,上位机编写控制程序对试验车进行控制(上位机控制程序后续会讲到)。遥控器直接控制的方式很简单,就不多讲了。
图3 试验车电机驱动器和遥控器参数
2、试验车通讯设计
由于一直以来都是跟着《古月居》学习ROS系统,所以本次试验车的通讯设计,打算采用ROS作为平台。在试验车上,TX2是大脑(主控板),其他的传感器基本用来感知周围环境。接下来我们从简入难,首先通过传感器的通讯协议进行数据解析,然后编写launch文件,将所有传感器全部调用起来,使用PYQT设计上位机进行显示,最终实现试验车的自主巡检、构建三维场景、目标检测识别等任务。
2.1 ROS工作环境创建
2.1.1 创建工作空间与功能包
图片摘自:古月居《ROS入门21讲》课件PPT
2.2 12V10Ah锂电池报文解析
进入上面创建的工作环境中
cd ~/catkin_ws/src
在src文件夹下,创建power12v10ah_publisher.py文件,将下面代码复制进文件中 (代码前面的空格,有一部分出现不兼容现象,需要手动调整一下)
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
该例程将发布/power12v10ah话题,自定义消息类型power12v10ah::power12v
12V10AH为USB2
12V20AH为USB0
IMU为USB1
"""
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 power12v_10ah
class CAR_CONTROL_TO_ROS():
def __init__(self):
# ROS节点初始化
rospy.init_node('power12v10ah_node', anonymous=True)
# 打开12v10ah锂电池串口
self.battery_12v10ah_open()
# 电机参数设置
self.motor_Front_left_L = 0x00
self.motor_Front_left_H = 0x00
self.motor_Front_right_L = 0x00
self.motor_Front_right_H = 0x00
self.motor_back_left_L = 0x00
self.motor_back_left_H = 0x00
self.motor_back_right_L = 0x00
self.motor_back_right_H = 0x00
# 开启新线程发送和接收12v10ah锂电池发来的数据,使用msg消息方式
# 还可以接收驱动电机发送的数据,并进行控制电机
write_motor_thread = threading.Thread(target=self.motor_send) # type: Thread
write_motor_thread.start()
motor_recv_thread = threading.Thread(target=self.motor_recv) # type: Thread
motor_recv_thread.start()
# 创建一个Publisher,发布名为/power12v10ah的topic,消息类型为car_207::power12v_10ah,队列长度10
self.power12v_10ah_info_pub = rospy.Publisher('/power12v10ah', power12v_10ah, queue_size=10)
#设置循环的频率
rate = rospy.Rate(10)
self.idx = 0
while not rospy.is_shutdown():
self.idx += 1
# # 按照循环频率延时
rate.sleep()
# 打开串口
def battery_12v10ah_open(self):
try:
self.serial_12v10ah.close()
except:
pass
try:
self.serial_12v10ah = serial.Serial('/dev/ttyUSB2', 115200, timeout=4) # com_kou
print("serial_12v10ah.name", self.serial_12v10ah.name)
#if serial_12v10ah.isOpen():
# print('serial_12v10ah 串口已打开')
#else:
# print('serial_12v10ah 串口打开失败')
except:
print("serial_12v10ah RESTART_ERROR")
# 给电机发送数据
def motor_send(self): #从串口获取数据
while 1:
try:
# TODO:现在给电机发送的扭矩全部为0,使用按键控制小车的功能暂未添加
motor_send_check_full = 0xfe + 0xef + 0x08 + self.motor_Front_left_H + self.motor_Front_left_L + self.motor_Front_right_H + self.motor_Front_right_L + self.motor_back_left_H + self.motor_back_left_L + self.motor_back_right_H + self.motor_back_right_L
motor_send_check = motor_send_check_full & 0xff
# feef08000afff6000afff6f3
motor_send = [0xfe, 0xef, 0x08, self.motor_Front_left_H, self.motor_Front_left_L, self.motor_Front_right_H, self.motor_Front_right_L,
self.motor_back_left_H, self.motor_back_left_L, self.motor_back_right_H, self.motor_back_right_L, motor_send_check]
motor_send_byte = bytes(motor_send)
self.serial_12v10ah.write(motor_send_byte)
# motor_recv_thread = threading.Thread(target=self.motor_recv) # type: Thread
# motor_recv_thread.start()
except:
print('给电机发送指令 失败')
time.sleep(0.3) # 不要超过500ms
# 电机接受数据校验位
def motor_recv_check(self, motor_recv_data):
motor_recv_sum = 0
for i in range(len(motor_recv_data)-1):
motor_recv_sum += motor_recv_data[i]
# print('a', motor_recv_data[-1])
# print("b",motor_recv_sum & 0xff)
return motor_recv_sum & 0xff
# 接收电机数据
def motor_recv(self):
while 1:
time.sleep(0.02)
try:
num = self.serial_12v10ah.inWaiting()
except:
pass
if num > 0:
data12v10ah = self.serial_12v10ah.read(num)
data_check = self.motor_recv_check(data12v10ah)
if len(data12v10ah) == 23 and data12v10ah[0] == 254 and data12v10ah[1] == 239 and data_check == data12v10ah[-1]:
# 解析接受的串口数据
voltage_12v10ah_ = (((data12v10ah[-3] << 8) + data12v10ah[-2]) * 100)
power_12v10ah_ = data12v10ah[-4]
# 消息类型为power12v_10ah,其实就是msg的文件名
power12v10ah_msg = power12v_10ah()
power12v10ah_msg.name = "power12v10ah_value"
power12v10ah_msg.voltage = voltage_12v10ah_
power12v10ah_msg.current = 0
power12v10ah_msg.battery_per = power_12v10ah_
# 发布消息
self.power12v_10ah_info_pub.publish(power12v10ah_msg)
rospy.loginfo("Publsh power12v10ah message[%s, %.3f, %.3f, %.3f]",
power12v10ah_msg.name, power12v10ah_msg.voltage,
power12v10ah_msg.current, power12v10ah_msg.battery_per)
# print("data12v10ah:", list(data12v10ah))
# print("电池电量为:{}%".format(data12v10ah[-4]))
# print("电池电压为:{}mv".format(((data12v10ah[-3] << 8) + data12v10ah[-2]) * 100)) # unit:mv
# self.lcdNumber.display(data12v10ah[-4])
# self.lcdNumber_2.display(((data12v10ah[-3] << 8) + data12v10ah[-2]) * 100)
else:
print("12V10AH电池接收数据错误")
# 全部退出程序
def quit(self):
self.serial_12v10ah.close()
os._exit(0)
if __name__ == '__main__':
try:
car_control_full = CAR_CONTROL_TO_ROS()
except rospy.ROSInterruptException:
pass
接下来返回到~/catkin_ws文件夹下
cd ..
然后在terminal1中运行
roscore
在terminal2中运行
source devel/setup.bash
catkin_make
rosrun test_pkg power12v10ah_publisher.py
OK,现在应该可以看到ROS一直发布锂电池电压、剩余电量 、电流等信息。 (由于实验设备不在身边,所以就先不放实际的运行效果图了)
2.3 12V20Ah锂电池报文解析
下面根据12V20Ah锂电池的通讯协议,对其报文进行解析。报文解析方式与上述12v10Ah基本相同,所以这一节快速瞄一眼就可以了。 1、回到上一节我们创建的test_pkg工作空间下 2、首先在src文件夹下,创建power12v20ah_publisher.py文件,将下面代码复制进文件中
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
该例程将发布/power12v20ah话题,自定义消息类型power12v20ah::power12v
"""
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 power12v_20ah
class CAR_CONTROL_TO_ROS():
def __init__(self):
# ROS节点初始化
rospy.init_node('power12v20ah_node', anonymous=True)
# 打开12v20ah锂电池串口
self.battery_12v20ah_open()
# 开启新线程发送和接收12v20ah锂电池发来的数据,使用msg消息方式
write12v20ahmsg_thread = threading.Thread(target=self.write12v20ah) # type: Thread
write12v20ahmsg_thread.start()
read12v20ahmsg_thread = threading.Thread(target=self.read12v20ah) # type: Thread
read12v20ahmsg_thread.start()
# 创建一个Publisher,发布名为/power12v20ah的topic,消息类型为car_207::power12v_20ah,队列长度10
self.power12v_info_pub = rospy.Publisher('/power12v20ah', power12v_20ah, queue_size=10)
#设置循环的频率
rate = rospy.Rate(5)
self.idx = 0
while not rospy.is_shutdown():
self.idx += 1
# # 按照循环频率延时
rate.sleep()
# 读取12V20AH锂电池信息
def write12v20ah(self): #从串口获取数据
while 1:
try:
# TODO:
write12v20ahmsg = [0x01, 0x04, 0x31, 0x00, 0x00, 0x1A, 0x7F, 0x3d]
# print('motorsend:', write12v20ahmsg)
write12v20ahmsg_byte = bytes(write12v20ahmsg)
self.serial_12v20ah.write(write12v20ahmsg_byte)
print("发送数据为:", list(write12v20ahmsg_byte))
# read12v20ahmsg_thread = threading.Thread(target=self.read12v20ah) # type: Thread
# read12v20ahmsg_thread.start()
# self.read12v20ahmsg()
# print("succeed_send_data")
except:
rospy.loginfo("给电机发送指令 %s .", '失败')
# print('给电机发送指令失败')
time.sleep(0.4) # 不要超过500ms
# 接收电机数据
def read12v20ah(self):
while 1:
try:
if self.serial_12v20ah.isOpen():
# print('串口已打开')
time.sleep(0.5)
num = self.serial_12v20ah.inWaiting()
except:
pass
print("num", num)
if num > 0:
data12v20ah = self.serial_12v20ah.read(num)
# print(len(data))
# print(data)
if len(data12v20ah) == 57:
power_12v20ah_ = (data12v20ah[9] << 8) + data12v20ah[10]
voltage_12v20ah_ = (((data12v20ah[3] << 8) + data12v20ah[4]) << 8 ) + ((data12v20ah[5] << 8) + data12v20ah[6])
# rospy.loginfo("12V20AH电池剩余电量 %s ,电压 %s.", power_12v20ah_, voltage_12v20ah_)
power12v20ah_msg = power12v_20ah()
power12v20ah_msg.name = "power12v20ah_value"
power12v20ah_msg.voltage = voltage_12v20ah_
power12v20ah_msg.current = 0
power12v20ah_msg.battery_per = power_12v20ah_
# 发布消息
self.power12v_info_pub.publish(power12v20ah_msg)
rospy.loginfo("Publsh power12v20ah message[%s, %.3f, %.3f, %.3f]",
power12v20ah_msg.name, power12v20ah_msg.voltage,
power12v20ah_msg.current, power12v20ah_msg.battery_per)
# print('电量:', ((data[9] << 8) + data[10]))
# print('电压:', ((((data[3] << 8) + data[4]) << 8 ) + ((data[5] << 8) + data[6])))
else:
print("12V20aH电池接收数据 失败")
def read12v20ahmsg_check(self, read12v20ahmsg_data):
read12v20ahmsg_sum = 0
for i in range(len(read12v20ahmsg_data)-1):
read12v20ahmsg_sum += read12v20ahmsg_data[i]
# print('a', read12v20ahmsg_data[-1])
# print("b",read12v20ahmsg_sum & 0xff)
return read12v20ahmsg_sum & 0xff
# 打开串口
def battery_12v20ah_open(self):
# try:
# self.serial_12v20ah.close()
# except:
# pass
try:
self.serial_12v20ah = serial.Serial('/dev/ttyUSB0', 9600, timeout=10) # com_kou
print("self.serial_12v20ah.name", self.serial_12v20ah.name)
if self.serial_12v20ah.isOpen():
print('串口已打开')
else:
print('串口打开失败')
except:
print("12V20AH串口开启失败")
# 关闭串口
def battery_12v20ah_close(self):
try:
self.serial_12v20ah.close()
except:
print('检测不到串口 serial_12v20ah')
# 全部退出程序
def quit(self):
self.serial_12v20ah.close()
os._exit(0)
if __name__ == '__main__':
try:
car_control_full = CAR_CONTROL_TO_ROS()
except rospy.ROSInterruptException:
pass
接下来返回到~/catkin_ws文件夹下
cd ..
然后在terminal1中运行
roscore
在terminal2中运行
source devel/setup.bash
catkin_make
rosrun test_pkg power12v20ah_publisher.py