ROS底盘控制节点源码分析

先在机器人端通过launch文件启动底盘控制。

@robot:~$ roslaunch base_control base_control.launch
... logging to /home/jym/.ros/log/3e52acda-914a-11ec-beaa-ac8247315e93/roslaunch-robot-8759.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.started roslaunch server http://192.168.0.110:36751/SUMMARY
========PARAMETERS* /base_control/ackermann_cmd_topic: ackermann_cmd* /base_control/base_id: base_footprint* /base_control/battery_topic: battery* /base_control/baudrate: 115200* /base_control/cmd_vel_topic: cmd_vel* /base_control/imu_id: imu* /base_control/imu_topic: imu* /base_control/odom_id: odom* /base_control/odom_topic: odom* /base_control/port: /dev/move_base* /base_control/pub_imu: False* /base_control/pub_sonar: False* /base_control/sub_ackermann: False* /rosdistro: melodic* /rosversion: 1.14.10NODES/base_control (base_control/base_control.py)auto-starting new master
process[master]: started with pid [8769]
ROS_MASTER_URI=http://192.168.0.110:11311setting /run_id to 3e52acda-914a-11ec-beaa-ac8247315e93
process[rosout-1]: started with pid [8780]
started core service [/rosout]
process[base_control-2]: started with pid [8783]
[INFO] [1645250866.687446]: NanoCar_Pro base control ...
[INFO] [1645250866.713360]: Opening Serial
[INFO] [1645250866.716848]: Serial Open Succeed
[INFO] [1645250867.309188]: Move Base Hardware Ver 2.1.0,Firmware Ver 2.1.3
[INFO] [1645250867.378238]: SN:002b00413138511532323338
[INFO] [1645250867.385795]: Type:RC_ACKERMAN Motor:RS365 Ratio:11.0 WheelDiameter:72.0

检查话题列表，看目前ros中有哪些话题。

@robot:~$ rostopic list
/battery
/cmd_vel
/odom
/rosout
/rosout_agg
/tf

rostopic is a command-line tool for printing information about ROS Topics.Commands:rostopic bw   display bandwidth used by topicrostopic delay   display delay of topic from timestamp in headerrostopic echo    print messages to screenrostopic find   find topics by typerostopic hz  display publishing rate of topic    rostopic info   print information about active topicrostopic list   list active topicsrostopic pub  publish data to topicrostopic type  print topic or field type

battery话题，输出的内容有电池的电压和电流，发布者是机器人底盘。

@robot:~$ rostopic info /battery
Type: sensor_msgs/BatteryStatePublishers: * /base_control (http://192.168.0.110:38535/)Subscribers: None@robot:~$  rostopic echo /battery
header: seq: 1stamp: secs: 1645251153nsecs: 739257097frame_id: "base_footprint"
voltage: 12.0539999008
current: 0.670000016689
charge: 0.0
capacity: 0.0
design_capacity: 0.0
percentage: 0.0
power_supply_status: 0
power_supply_health: 0
power_supply_technology: 0
present: False
cell_voltage: []
location: ''
serial_number: ''
---

cmd_vel话题，订阅者是底盘

@robot:~$ rostopic info /cmd_vel
Type: geometry_msgs/TwistPublishers: NoneSubscribers: * /base_control (http://192.168.0.110:38535/)

odom话题，里程计，发布者是机器人底盘。

@robot:~$ rostopic info /odom
Type: nav_msgs/OdometryPublishers: * /base_control (http://192.168.0.110:38535/)Subscribers: None

odom话题，输出的主要信息是位置坐标，航向、线加速度、角加速度。

@robot:~$ rostopic echo /odom
header: seq: 1stamp: secs: 1645251339nsecs: 379389047frame_id: "odom"
child_frame_id: "base_footprint"
pose: pose: position: x: 0.0y: 0.0z: 0.0orientation: x: 0.0y: 0.0z: 0.0w: 1.0covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: twist: linear: x: 0.0y: 0.0z: 0.0angular: x: 0.0y: 0.0z: -0.006covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

然后在机器人端终止运行。输出下面这些提示。

^C[base_control-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

初始化

下面看一下底盘控制节点的可执行文件base_control.py里面的BaseControl类，类里面有一些初始化。

为什么订阅器不需要定时器而发布器需要定时器。

因为订阅器是通过话题来驱动的，收到了订阅的话题之后，会直接跳转到回调函数里面，不需要定时器定期来执行。

关于timer_communication：

用于检查现在nano是否收到stm32的消息。定时器时间到了就去执行timerCommunicationCB回调函数。

def __init__(self):#Get params，获取各种参数，获取的参数就是launch文件中param标签设置的参数。#如果没有在 launch 文件中获取到这些参数就会使用类中提前设置的默认值。self.baseId = rospy.get_param('~base_id','base_footprint')self.odomId = rospy.get_param('~odom_id','odom')self.device_port = rospy.get_param('~port','/dev/ttyUSB0')self.baudrate = int(rospy.get_param('~baudrate','115200'))self.odom_freq = int(rospy.get_param('~odom_freq','50'))self.odom_topic = rospy.get_param('~odom_topic','/odom')self.battery_topic = rospy.get_param('~battery_topic','battery')self.battery_freq = float(rospy.get_param('~battery_freq','1'))self.cmd_vel_topic= rospy.get_param('~cmd_vel_topic','/cmd_vel')self.ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic','/ackermann_cmd_topic')#如果在启动时设置底盘节点launch文件pub_imu参数为True，底盘节点接收launch文件中param标签的参数，然后设置imu变量信息self.pub_imu = bool(rospy.get_param('~pub_imu',False))if(self.pub_imu == True):self.imuId = rospy.get_param('~imu_id','imu')self.imu_topic = rospy.get_param('~imu_topic','imu')self.imu_freq = float(rospy.get_param('~imu_freq','50'))if self.imu_freq > 100:self.imu_freq = 100self.pub_sonar = bool(rospy.get_param('~pub_sonar',False))self.sub_ackermann = bool(rospy.get_param('~sub_ackermann',False))#define param，BaseControl类使用到的变量的定义self.current_time = rospy.Time.now()self.previous_time = self.current_timeself.pose_x = 0.0self.pose_y = 0.0self.pose_yaw = 0.0self.serialIDLE_flag = 0self.trans_x = 0.0self.trans_y = 0.0self.rotat_z = 0.0self.speed = 0.0self.steering_angle = 0.0self.sendcounter = 0self.ImuErrFlag = Falseself.EncoderFlag = Falseself.BatteryFlag = Falseself.OdomTimeCounter = 0self.BatteryTimeCounter = 0self.Circleloop = queue(capacity = 1024*4)#注意这个环形队列，存放串口中缓存的数据self.Vx = 0self.Vy = 0self.Vyaw = 0self.Yawz = 0self.Vvoltage = 0self.Icurrent = 0self.Gyro = [0,0,0]self.Accel = [0,0,0]self.Quat = [0,0,0,0]self.Sonar = [0,0,0,0]self.movebase_firmware_version = [0,0,0]self.movebase_hardware_version = [0,0,0]self.movebase_type = ["NanoCar","NanoRobot","4WD_OMNI","4WD"]self.motor_type = ["25GA370","37GB520"]self.last_cmd_vel_time = rospy.Time.now()self.last_ackermann_cmd_time = rospy.Time.now()# Serial Communication，串口通信，打开NANO串口并连接到stm32上try:self.serial = serial.Serial(self.device_port,self.baudrate,timeout=10)rospy.loginfo("Opening Serial")try:if self.serial.in_waiting:self.serial.readall()except:rospy.loginfo("Opening Serial Try Faild")passexcept:rospy.logerr("Can not open Serial"+self.device_port)self.serial.closesys.exit(0)rospy.loginfo("Serial Open Succeed")#if move base type is ackermann car like robot and use ackermann msg ,sud ackermann topic,else sub cmd_vel topic#不同机器人结构类型下订阅器的定义if(('NanoCar' in base_type) & (self.sub_ackermann == True)):#设置为阿克曼结构类型，在launch文件中默认sub_ackermann为Falsefrom ackermann_msgs.msg import AckermannDriveStampedself.sub = rospy.Subscriber(self.ackermann_cmd_topic,AckermannDriveStamped,self.ackermannCmdCB,queue_size=20)else:#否则，订阅器订阅消息类型为 Twist，接收话题为 cmd_vel，收到话题就会进入 cmdCB 函数self.sub = rospy.Subscriber(self.cmd_vel_topic,Twist,self.cmdCB,queue_size=20)#queue_size参数是在ROS Hydro及更新版本中新增的，用于在订阅者接收消息的速度不够快的情况下，限制排队的消息数量。self.pub = rospy.Publisher(self.odom_topic,Odometry,queue_size=10)self.battery_pub = rospy.Publisher(self.battery_topic,BatteryState,queue_size=3)if self.pub_sonar:#超声波消息类型的发布if sonar_num > 0:self.timer_sonar = rospy.Timer(rospy.Duration(100.0/1000),self.timerSonarCB)self.range_pub1 = rospy.Publisher('sonar_1',Range,queue_size=3)if sonar_num > 1:    self.range_pub2 = rospy.Publisher('sonar_2',Range,queue_size=3)if sonar_num > 2:self.range_pub3 = rospy.Publisher('sonar_3',Range,queue_size=3)if sonar_num > 3:self.range_pub4 = rospy.Publisher('sonar_4',Range,queue_size=3)self.tf_broadcaster = tf.TransformBroadcaster()#tf坐标发布器,实时发布坐标偏移量#发布器 根据频率定期执行话题发布任务self.timer_odom = rospy.Timer(rospy.Duration(1.0/self.odom_freq),self.timerOdomCB)#里程计self.timer_battery = rospy.Timer(rospy.Duration(1.0/self.battery_freq),self.timerBatteryCB)  self.timer_communication = rospy.Timer(rospy.Duration(1.0/500),self.timerCommunicationCB)#与stm32通讯的定时器#inorder to compatibility old version firmware,imu topic is NOT pud in defaultif(self.pub_imu):#imu 的发布器和定时器self.timer_imu = rospy.Timer(rospy.Duration(1.0/self.imu_freq),self.timerIMUCB) self.imu_pub = rospy.Publisher(self.imu_topic,Imu,queue_size=10)self.getVersion()#获取底盘软硬件版本号#move base imu initialization need about 2s,during initialization,move base system is blocked#so need this gap#获取通讯协议(因为刚才获取了底盘软硬件版本号)与底盘建立连接后会初始化imu，在此期间不接收数据所以需要这个延迟#为了保证此时没有大量数据传到底盘的通讯队列里面time.sleep(2.2)self.getSN()#查询主板 SN 号，这里只是发出去了还没有收到回复time.sleep(0.01)self.getInfo()#获取底盘配置信息

注：代码里的Timer：

rospy.Timer(period,callback,oneshot=False)
period，调用回调函数的时间间隔，如rospy.Duration(0.1)即为10分之1秒
callback，回调函数的定义
oneshot，定时器，是否多次执行。false即一直执行。Timer实例会在每一个时间间隔，调用一次my_callback。

def my_callback(event):print 'Timer called at' + str(event.current_real)rospy.Timer(rospy.Duration(2),my_callback)

自定义通讯协议

通讯协议数据构成：串口波特率：115200，1停止位，8数据位，无校验。

约定:

0.帧头：1Byte 0x5a 固定值

1.功能码：1Byte控制板端(stm32)发送的功能码为偶数,控制板端(stm32)接收的功能码为奇数

2.帧长度：整个数据包长度，包括从帧头到校验码全部数据。

帧头(1 Byte)+帧长度(1 Byte)+ID(1 Byte)+功能码(1 Byte)+数据(0~250Byte))+预留位(1 Byte)+CRC校验(1 Byte)

3.ID：下位机编号，为级联设计预留。

4.预留位，设置为0x00，为后续协议扩展预留。

5.CRC校验：1Byte，校验方式为CRC-8/MAXIM，设置为0xFF，则强制不进行CRC校验。

6.数据：长度和内容具体参照各功能码定义。

7.线速度单位为m/s，角速度单位为rad/s（弧度制），角度单位为度（角度制）。

nano-stm32建立连接定义：

在未建立连接的状态下，stm32收到协议内数据，则判定为建立连接，建立连接时，stm32IMU会执行初始化，耗时2S左右。

nano-stm32断开连接定义：

在建立连接状态后，超过1000ms没有收到新的协议内数据，stm32判定断开连接，主动停止电机运动。

其中一部分功能码如下，根据不同的功能码，读取不同的值。

0x01
上位机向下位机发送速度控制指令，数据长度为6Byte，数据为X轴方向速度*1000(int16_t) + Y轴方向速度*1000(int16_t) + Z轴角速度*1000(int16_t)
数据格式为：
Byte1   Byte2   Byte3   Byte4   Byte5   Byte6
X MSB   X LSB   Y MSB   Y LSB   Z MSB   Z LSB
例:5A 0C 01 01 01 F4 00 00 00 00 00 56 (底盘以0.5m/s的速度向前运动)
注：500=0x01F4；12=0x0c(databuf[3] == 0x14):#下位机上报IMU数据GyroX*100000(int32_t)、GyroY*100000(int32_t)、GyroZ*100000(int32_t)、
AccelX*100000(int32_t)、AccelY*100000(int32_t)、AccelZ*100000(int32_t)、QuatW×10000、QuatX×10000、QuatY×10000、QuatZ×100000x22
下位机回复配置信息
BASE_TYPE(底盘类型uint8_t) + MOTOR_TYPE(电机型号uint8_t) + ratio*10(电机减速比int16_t) + diameter*10(轮胎直径int16_t)

#class queue is design for uart receive data cache
#环形队列的读出和存储，做通讯协议解析时候，防止未及时读信息导致信息丢失。
class queue:def __init__(self, capacity = 1024*4):self.capacity = capacityself.size = 0self.front = 0self.rear = 0self.array = [0]*capacitydef is_empty(self):return 0 == self.sizedef is_full(self):return self.size == self.capacitydef enqueue(self, element):if self.is_full():raise Exception('queue is full')self.array[self.rear] = elementself.size += 1self.rear = (self.rear + 1) % self.capacitydef dequeue(self):if self.is_empty():raise Exception('queue is empty')self.size -= 1self.front = (self.front + 1) % self.capacitydef get_front(self):return self.array[self.front]def get_front_second(self):return self.array[((self.front + 1) % self.capacity)]def get_queue_length(self):return (self.rear - self.front + self.capacity) % self.capacitydef show_queue(self):for i in range(self.capacity):print self.array[i],print(' ')

#CRC-8 Calculate  crc计算函数用来做通讯校验def crc_1byte(self,data):crc_1byte = 0for i in range(0,8):if((crc_1byte^data)&0x01):crc_1byte^=0x18crc_1byte>>=1crc_1byte|=0x80else:crc_1byte>>=1data>>=1return crc_1bytedef crc_byte(self,data,length):ret = 0for i in range(length):ret = self.crc_1byte(ret^data[i])return ret

base_control节点将订阅的cmd_vel信息通过串口或其它通信接口发送给下位机（嵌入式控制板）。

#Subscribe vel_cmd call this to send vel cmd to move base
#cmdCB 会把接收到的话题的关键信息取出来，根据通信协议做一个转换。再通过串口发送到底盘上，实现对底盘的控制def cmdCB(self,data):self.trans_x = data.linear.xself.trans_y = data.linear.yself.rotat_z = data.angular.zself.last_cmd_vel_time = rospy.Time.now()output = chr(0x5a) + chr(12) + chr(0x01) + chr(0x01) + \chr((int(self.trans_x*1000.0)>>8)&0xff) + chr(int(self.trans_x*1000.0)&0xff) + \chr((int(self.trans_y*1000.0)>>8)&0xff) + chr(int(self.trans_y*1000.0)&0xff) + \chr((int(self.rotat_z*1000.0)>>8)&0xff) + chr(int(self.rotat_z*1000.0)&0xff) + \chr(0x00)outputdata = [0x5a,0x0c,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00]outputdata[4] = (int(self.trans_x*1000.0)>>8)&0xffoutputdata[5] = int(self.trans_x*1000.0)&0xffoutputdata[6] = (int(self.trans_y*1000.0)>>8)&0xffoutputdata[7] = int(self.trans_y*1000.0)&0xffoutputdata[8] = (int(self.rotat_z*1000.0)>>8)&0xffoutputdata[9] = int(self.rotat_z*1000.0)&0xffcrc_8 = self.crc_byte(outputdata,len(outputdata)-1)output += chr(crc_8)while self.serialIDLE_flag:time.sleep(0.01)self.serialIDLE_flag = 4try:while self.serial.out_waiting:passself.serial.write(output)except:rospy.logerr("Vel Command Send Faild")self.serialIDLE_flag = 0

#Communication Timer callback to handle receive data，通讯定时器回调函数#depend on communication protocoldef timerCommunicationCB(self,event):length = self.serial.in_waitingif length:#当定时时间到，检查串口中是否有缓存的数据。有则读取，存放到环形消息队列中reading = self.serial.read_all()if len(reading)!=0:for i in range(0,len(reading)):data = (int(reading[i].encode('hex'),16)) try:self.Circleloop.enqueue(data)#把收到的消息读出来，防止缓存满，消息丢失except:passelse:pass#将环形队列中的内容重新读出来，所有stm32从串口发送到nano的数据都是在这个回调函数里做的处理if self.Circleloop.is_empty()==False:data = self.Circleloop.get_front()if data == 0x5a:#如果读出来的数据是数据帧的帧头就获取数据长度length = self.Circleloop.get_front_second()#帧长度if length > 1 :if self.Circleloop.get_front_second() <= self.Circleloop.get_queue_length():databuf = []for i in range(length):databuf.append(self.Circleloop.get_front())self.Circleloop.dequeue()if (databuf[length-1]) == self.crc_byte(databuf,length-1):passelse:pass#parse receive data，根据功能码设置读取不同的值，下面功能码都是偶数。意味着stm32向nano发送数据if(databuf[3] == 0x04):#下位机上报当前速度，数据长度为6Byteself.Vx =    databuf[4]*256self.Vx +=   databuf[5]self.Vy =    databuf[6]*256self.Vy +=   databuf[7]self.Vyaw =  databuf[8]*256self.Vyaw += databuf[9]elif(databuf[3] == 0x06):#下位机上报当IMU数据self.Yawz =  databuf[8]*256self.Yawz += databuf[9]elif (databuf[3] == 0x08):#下位机上报电池信息，电压Voltage*1000(uint16_t) + 电流Current*1000(uint16_t)self.Vvoltage = databuf[4]*256self.Vvoltage += databuf[5]self.Icurrent = databuf[6]*256self.Icurrent += databuf[7]elif (databuf[3] == 0x0a):#下位机上报速度航向信息，线速度*1000、角度*100、角速度*1000self.Vx =    databuf[4]*256self.Vx +=   databuf[5]self.Yawz =  databuf[6]*256self.Yawz += databuf[7]self.Vyaw =  databuf[8]*256self.Vyaw += databuf[9]elif (databuf[3] == 0x12):#下位机上报速度航向信息，X轴线速度*1000、Y轴线速度*1000、角度*100、角速度*1000self.Vx =    databuf[4]*256self.Vx +=   databuf[5]self.Vy =  databuf[6]*256self.Vy += databuf[7]self.Yawz =  databuf[8]*256self.Yawz += databuf[9]    self.Vyaw =  databuf[10]*256self.Vyaw += databuf[11]                          elif (databuf[3] == 0x14):#下位机上报IMU数据self.Gyro[0] = int(((databuf[4]&0xff)<<24)|((databuf[5]&0xff)<<16)|((databuf[6]&0xff)<<8)|(databuf[7]&0xff))self.Gyro[1] = int(((databuf[8]&0xff)<<24)|((databuf[9]&0xff)<<16)|((databuf[10]&0xff)<<8)|(databuf[11]&0xff))self.Gyro[2] = int(((databuf[12]&0xff)<<24)|((databuf[13]&0xff)<<16)|((databuf[14]&0xff)<<8)|(databuf[15]&0xff))self.Accel[0] = int(((databuf[16]&0xff)<<24)|((databuf[17]&0xff)<<16)|((databuf[18]&0xff)<<8)|(databuf[19]&0xff))self.Accel[1] = int(((databuf[20]&0xff)<<24)|((databuf[21]&0xff)<<16)|((databuf[22]&0xff)<<8)|(databuf[23]&0xff))self.Accel[2] = int(((databuf[24]&0xff)<<24)|((databuf[25]&0xff)<<16)|((databuf[26]&0xff)<<8)|(databuf[27]&0xff))self.Quat[0] = int((databuf[28]&0xff)<<8|databuf[29])self.Quat[1] = int((databuf[30]&0xff)<<8|databuf[31])self.Quat[2] = int((databuf[32]&0xff)<<8|databuf[33])self.Quat[3] = int((databuf[34]&0xff)<<8|databuf[35])elif (databuf[3] == 0x1a):self.Sonar[0] = databuf[4]self.Sonar[1] = databuf[5]self.Sonar[2] = databuf[6]self.Sonar[3] = databuf[7]    elif(databuf[3] == 0xf2):#下位机回复版本号，硬件版本号xx.yy.zz,软件版本号aa.bb.ccself.movebase_hardware_version[0] = databuf[4]self.movebase_hardware_version[1] = databuf[5]self.movebase_hardware_version[2] = databuf[6]self.movebase_firmware_version[0] = databuf[7]self.movebase_firmware_version[1] = databuf[8]self.movebase_firmware_version[2] = databuf[9]version_string = "Move Base Hardware Ver %d.%d.%d,Firmware Ver %d.%d.%d"\%(self.movebase_hardware_version[0],self.movebase_hardware_version[1],self.movebase_hardware_version[2],\self.movebase_firmware_version[0],self.movebase_firmware_version[1],self.movebase_firmware_version[2])rospy.loginfo(version_string)elif(databuf[3] == 0xf4):#下位机回复SN号sn_string = "SN:"for i in range(4,16):#数据长度为12Byte,高位在前，低位在后sn_string = "%s%02x"%(sn_string,databuf[i])rospy.loginfo(sn_string)                            elif(databuf[3] == 0x22):#下位机回复配置信息fRatio = float(databuf[6]<<8|databuf[7])/10fDiameter = float(databuf[8]<<8|databuf[9])/10info_string = "Type:%s Motor:%s Ratio:%.01f WheelDiameter:%.01f"\%(self.movebase_type[databuf[4]-1],self.motor_type[databuf[5]-1],fRatio,fDiameter)rospy.loginfo(info_string)else:passelse:passelse:self.Circleloop.dequeue()else:# rospy.loginfo("Circle is Empty")pass

通信协议和话题的转换

#Odom Timer call this to get velocity and imu info and convert to odom topicdef timerOdomCB(self,event):#使用串口给stm32发送一条消息来获取里程计信息#Get move base velocity dataif self.movebase_firmware_version[1] == 0: #old version firmware have no version info and not support new command belowoutput = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x09) + chr(0x00) + chr(0x38) #0x38 is CRC-8 valueelse:#nano向stm32获取里程计信息#in firmware version new than v1.1.0,support this command   output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x11) + chr(0x00) + chr(0xa2) while(self.serialIDLE_flag):time.sleep(0.01)self.serialIDLE_flag = 1try:while self.serial.out_waiting:passself.serial.write(output)except:rospy.logerr("Odom Command Send Faild")self.serialIDLE_flag = 0   #calculate odom data，#stm32返回的里程计信息在 timerCommunicationCB 函数中做解析后取得速度self.Vx赋值给局部变量Vx = float(ctypes.c_int16(self.Vx).value/1000.0)Vy = float(ctypes.c_int16(self.Vy).value/1000.0)Vyaw = float(ctypes.c_int16(self.Vyaw).value/1000.0)self.pose_yaw = float(ctypes.c_int16(self.Yawz).value/100.0)self.pose_yaw = self.pose_yaw*math.pi/180.0self.current_time = rospy.Time.now()dt = (self.current_time - self.previous_time).to_sec()self.previous_time = self.current_timeself.pose_x = self.pose_x + Vx * (math.cos(self.pose_yaw))*dt - Vy * (math.sin(self.pose_yaw))*dt#速度对时间做积分得出相对位移self.pose_y = self.pose_y + Vx * (math.sin(self.pose_yaw))*dt + Vy * (math.cos(self.pose_yaw))*dtpose_quat = tf.transformations.quaternion_from_euler(0,0,self.pose_yaw)        #通信协议和话题的一个转换msg = Odometry()#定义了一个里程计的消息类型msg.header.stamp = self.current_timemsg.header.frame_id = self.odomIdmsg.child_frame_id =self.baseIdmsg.pose.pose.position.x = self.pose_x#将前面计算出来的值给到消息中msg.pose.pose.position.y = self.pose_ymsg.pose.pose.position.z = 0msg.pose.pose.orientation.x = pose_quat[0]msg.pose.pose.orientation.y = pose_quat[1]msg.pose.pose.orientation.z = pose_quat[2]msg.pose.pose.orientation.w = pose_quat[3]msg.twist.twist.linear.x = Vxmsg.twist.twist.linear.y = Vymsg.twist.twist.angular.z = Vyawself.pub.publish(msg)#通过里程计发布器发布出去，之前定义过pubself.tf_broadcaster.sendTransform((self.pose_x,self.pose_y,0.0),pose_quat,self.current_time,self.baseId,self.odomId)

#Battery Timer callback function to get battery infodef timerBatteryCB(self,event):#给stm32发送查询电池信息的指令output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x07) + chr(0x00) + chr(0xe4) #0xe4 is CRC-8 valuewhile(self.serialIDLE_flag):time.sleep(0.01)self.serialIDLE_flag = 3try:while self.serial.out_waiting:passself.serial.write(output)except:rospy.logerr("Battery Command Send Faild")self.serialIDLE_flag = 0msg = BatteryState()#之所以没有对获取的数据做解析，是因为解析已经在timerCommunicationCB函数里面进行了msg.header.stamp = self.current_time#获取到的数值传输到电池类型的消息中msg.header.frame_id = self.baseIdmsg.voltage = float(self.Vvoltage/1000.0)msg.current = float(self.Icurrent/1000.0)self.battery_pub.publish(msg)#通过发布器发布

nano向stm32要东西的回调函数运行流程：nano先给stm32发送指令，给stm32说，我要你的东西。stm32传回的数据由timerCommunicationCB函数解析处理，回调函数将获取的处理后的值传到消息中，然后通过发布器发出去。

#main function
if __name__=="__main__":try:rospy.init_node('base_control',anonymous=True)if base_type != None:rospy.loginfo('%s base control ...'%base_type)else:rospy.loginfo('base control ...')rospy.logerr('PLEASE SET BASE_TYPE ENV FIRST')bc = BaseControl()#执行这个类rospy.spin()#循环执行except KeyboardInterrupt:bc.serial.closeprint("Shutting down")

坐标变换

tf_broadcaster.sendTransform是一个静态变换，因为机器人imu和底盘固定，转换关系就固定。

    #IMU Timer callback function to get raw imu infodef timerIMUCB(self,event):output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x13) + chr(0x00) + chr(0x33) #0x33 is CRC-8 valuewhile(self.serialIDLE_flag):time.sleep(0.01)self.serialIDLE_flag = 3try:while self.serial.out_waiting:passself.serial.write(output)except:rospy.logerr("Imu Command Send Faild")self.serialIDLE_flag = 0msg = Imu()msg.header.stamp = self.current_timemsg.header.frame_id = self.imuIdmsg.angular_velocity.x = float(ctypes.c_int32(self.Gyro[0]).value/100000.0)msg.angular_velocity.y = float(ctypes.c_int32(self.Gyro[1]).value/100000.0)msg.angular_velocity.z = float(ctypes.c_int32(self.Gyro[2]).value/100000.0)msg.linear_acceleration.x = float(ctypes.c_int32(self.Accel[0]).value/100000.0)msg.linear_acceleration.y = float(ctypes.c_int32(self.Accel[1]).value/100000.0)msg.linear_acceleration.z = float(ctypes.c_int32(self.Accel[2]).value/100000.0)msg.orientation.w = float(ctypes.c_int16(self.Quat[0]).value/10000.0)msg.orientation.x = float(ctypes.c_int16(self.Quat[1]).value/10000.0)msg.orientation.y = float(ctypes.c_int16(self.Quat[2]).value/10000.0)msg.orientation.z = float(ctypes.c_int16(self.Quat[3]).value/10000.0)self.imu_pub.publish(msg)  # TF value calculate from mechanical structure#读取车型，执行坐标变换。坐标作为话题发补出去，需要时间戳current_timeif('NanoRobot' in base_type):#第一个三个参数是距离变换，第二个是四元数的xyzw，角度变换self.tf_broadcaster.sendTransform((-0.062,-0.007,0.08),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId)   elif('NanoCar' in base_type):#imuId子坐标，baseId父坐标，从子坐标到父坐标的转换self.tf_broadcaster.sendTransform((0.0,0.0,0.09),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId) elif('4WD' in base_type):self.tf_broadcaster.sendTransform((-0.065,0.0167,0.02),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId)     else:pass

def timerOdomCB(self,event):#这个是个动态转换，传入的值是一个变量self.tf_broadcaster.sendTransform((self.pose_x,self.pose_y,0.0),pose_quat,self.current_time,self.baseId,self.odomId)

setbase.sh

定义机器人底盘类型，写到命令行里面，后面是带了个参数，参数就作为环境变量写入~/.bashrc 文件。

~代表用户主目录。

#!/bin/bashBASE_TYPE=$1
echo "export BASE_TYPE=$BASE_TYPE" >> ~/.bashrc
source ~/.bashrc

$ ./setbase.sh abc
bashrc 文件里面：出现同名的环境变量时只生效文件最末尾的
export BASE_TYPE=abc

udev 规则

进行端口 udev 规则转换(重命名)。

树莓派与机器人连接使用硬件串口，端口名无法修改。脚本会将 ttyS0 创建一个符号链接，move_base 链接到 ttyS0 ，赋予它所有用户可读可写权限。(重命名为move_base 并修改权限为可读写，MODE=0666表示可读写)

第一句话是指明脚本的解释器。

第二句话是输入到/etc/udev/rules.d/move_base_pi4.rules这个文件里面

#!/bin/bashecho  'KERNEL=="ttyS0", MODE:="0666", GROUP:="dialout",  SYMLINK+="move_base"' >/etc/udev/rules.d/move_base_pi4.rulessystemctl daemon-reload
service udev reload
sleep 1
service udev restart

统一端口名称。如果使用的是 USB 转串口的芯片，连接时端口名不确定。有可能是 ttyUSB0 也有可能是 ttyUSB1，多个设备通过usb连主机，很难分辨各个端口名所对应的设备。可以给USB 转串口的芯片设定规则，根据芯片的 Vender ID 和 Product ID 进行筛选。

给底盘使用的CH340芯片创建一个规则：端口名称是ttyUSB开头。

#!/bin/bashecho  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", GROUP:="dialout",  SYMLINK+="move_base"' >/etc/udev/rules.d/move_base_340.rulessystemctl daemon-reload
service udev reload
sleep 1
service udev restart

ls -l /dev/
move_base设备，会进行链接