先在機器人端通過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設備，會進行鏈接

總結

以上是生活随笔為你收集整理的ROS底盘控制节点 源码分析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。