6.1base_control功能包代码解析

好好学习28

已于 2024-04-19 11:11:14 修改

阅读量698

点赞数 16

文章标签：网络智能路由器

于 2024-04-19 11:10:31 首次发布

本文链接：https://blog.csdn.net/weixin_55877886/article/details/137958067

版权

6.1base_control功能包代码解析
本节内容主要讲解base_control功能包的代码内容和各个部分的作用
虚拟机中打开Visual Studio Code点击file->Open Folder打开工作空间，工作空间下找到src下的base_control功能包。

EXPLORER

CATKIN WS

.VSCODE

@__C_CPP_PROPERTIES.JSON

SETTINGS.JSON

BUILD

DEVEL

SRC

BASE_CONTROL

LAUNCH

SCRIPT

M CMAKELISTS.TXT

PACKAGE.XML

README.MD

BINADA PRACTICES

CMakeLists.txt文件：
CMakeLists.txt是创建功能包时自动产生的文件。由于base_control这个功能包的源码使用的python语言编写，也没有使用自定义消息或者服务类型，对这个文件的内容没有修改。
package.xml文件：
package.xml用来做功能包的描述，它内容包含有功能包名，版本号，功能包功能描述，作者联系邮箱，开源协议等等。

<?XML VERSION"1.0"?>

1234567

<NAME>BASE CONTROL</NAME>

<DESCRIPTION>THE BASE CONTROL PACKAGE</DESCRIPTION>

ONE MAINTAINER TAG REQUIRED, MULTIPLE ALLOWED, ONE PERSON PER TAG

EXAMPLE:

<MAINTAINER EMAIL三"LIU.FUZHIQWECHANGETECH.COM">FUZHI, LIU</MAINTAINER>

ONE LICENSE TAG REQUIRED, MULTIPLE ALLOWED, ONE LICENSE PER TAG -

BSD, MIT, BOOST SOFTWARE LICENSE, GPLV2, GPLV3, LGPLV2.1, LGPLV3

<!--

<LICENSE>APACHE 2.0</LICENSE>

文件末尾还包含一些run_depend标签，它是用来帮助rosdep这个工具解决依赖项的问题

DEPEND>ROSPY</RUN DEPEND>

<RUN

DEPEND>TF</RUN_DEPEND>

<RUN

DEPEND>GEOMETRY MSGS</RUN DEPEND>

<RUN

DEPEND>NAV_MSGS</RUN

DEPEND>

<RUN

_DEPEND>SENSOR_MSGS</RUN_DEPEND>

<RUN

DEPEND>ACKERMANN MSGS</RUN

DEPEND>

<RUN

_DEPEND>TELEOP_TWIST_KEYBOARD</RUN_DEPEND>

<RUN

<BUILDTOOL

_DEPEND>CATKIN</BUILDTOOL DEPEND>

launch文件夹：
打开launch文件夹，它下面只有一个base_control.launch文件

BASE_CONTROL.LAUNCH

SRC > BASE_CONTROL > LAUNCH >

<?XML VERSION"1.0"?>

BLAUNCHB

45678

<!- SUPPORT MULTI ROBOT

SUPPORT MULTI ROBOT

<!- ROBOT FRAME

DEFAULT-"BASE FOOTPRINT" />

NAME"BASE FRAME"

<ARG

DEFAULT"ODOM"/>

<ARG NAME:"ODOM_FRAME"

DEFAULT"IMU" />

'IMU FRAME"

<ARG

NAME

TOPIC

PUD

DEFAULT-"ODOM" /><!-- DO NOT USE '/' IN TOPIC 4

NAME"ODOM TOPIC"

<ARG

NAME

launch文件的第一行有一个<?xml version="1.0"?>它指明了一个xml的版本，launch文件是一个xml格式的文本launch文件的内容都写在launch标签里。
launch标签：

BASE_CONTROL.LAUNCH X

BASE_CONTROL.LAUNCH

SRC > BASE_CONTROL > LAUNCH >

<?XML VERSION三"1.0"?>

123N

>LAUNCHB

</LAUNCH>

launch标签规定了一片区域，所有的launch文件都由<launch>开头，由</launch>结尾，整个launch文件的内容都要写在<launch></launch>之间
arg标签：

1234567

<!- SUPPORT MULTI ROBOT

SUPPORT MULTI ROBOT

<!- ROBOT FRAME

DEFAULT-"BASE FOOTPRINT" />

<ARG NAME "BASE_FRAME"

DEFAULT"ODOM" />

NAMEODOMFRAME

<ARG N

DEFAULT"IMU" />

<ARG NAME"IMU_FRAME"

DEFAULT-"ODOM" />

<ARG NAME"ODOM TOPIC"

DEFAULT"IMU"/>

<ARG NAME"IMU TOPIC"

DEFAULT"BATTERY"/>

<ARG NAME"BATTERY TOPIC"

<!-- SUB TOPIC

<ARG NAME"CMD_VEL_TOPIC"

DEFAULT-"CMD VEL" /> <!-- DO NOT USE '/" IN TOPIC NAME-

DEFAULT:"ACKERMANN_CMD" /2

GARG NAME-"ACKERMANN_CMD_TOPIC"

<!- CONFIG PARAM

DEFAULT FALSE"/>

<ARG NAME"PUB IMU"

DEFAULT "FALSE" />

<ARG NAME"SUB ACKERMANN"

DEFAULT"FALSE" />

<ARG NAME "PUB SONAR"

L NODE

<!-BASE CONTROL R

<!--ROS NAMESPACE NAME CAN NOT BE EMPTY,SO NEED EVALUATE USE USE MULTI ROBOT OR NOT--7

<GROUP IF"$(EVAL ROBOT NAME -")")">...

arg标签是用来在launch文件中声明参数的，在launch中声明过的参数都可以在启动launch文件时传入已声明过的参数。我们在终端输入roslaunch base_control base_control.launch然后按TAB键补全就能看到可传入的参数列表。

BASE CONTROL BASE CONTROL.LAUNCH

BINGDA@UBUNTU:~$ ROSLAUNCH

ACKERMANN CMD TOPIC

VEL TOPIC

CMD

ODOM FRAME

PUB SONAR

IMU FRAME

ODOM TOPIC

BASE FRAME

ROBOT NAME

BATTERY TOPIC

IMU TOPIC

ACKERMANN

PUB IMU

SUB

default的内容则是对参数赋值。如无参数传入则使用launch文件中定义的值。也可以在启动launch文件时传入参数的值。例如传入pub_imu的值为true：
roslaunch base_control base_control.launch pub_imu:=ture
group标签：

Z!--ROS HAMESPACE NAME CAN NOL BE EMPTY ,SO NE

SO HEED EVALUATE USE MULTI ROBOT OR HOT-->

<GROUP IF-'$(EVAL ROBOT NAME

PKG-"BASE CONTROL" TYPE-"BASE CONTROL.PY" OUTPUT:"SCREEN""

<NODE NAME BASE CONTROL"

PARAM NAME BAUDRATE

VALUE115200"/>

VALUE-'/DEV/MOVE BASE'/>

PARAM NAME-"PORT"

VALUE-"$(ARG BASE FRAME)"/> <!-- BASE LINK NAME

<PARAM NAME BASE ID"

<!-- ODOM LINK NAME

VALUE"$(ARG ODOM_FRAME)"/>

PARAM NAME "ODOM ID"

<!-- IMU LINK NAME

VALUE-"$(ARG IMU_FRAME)"/>

<PARAM NAME "IMU ID"

<PARAM NAME三"ODOM TOPIC" VALUE-"S(ARG ODOM TOPIC)"/> <!---- TOPIC NAME

<PARAM NAME:"IMU TOPIC" VALUE三"S(ARG IMU TOPIC)"/> <!--- TOPIC NAME

<PARAM NAME三"DATTERY TOPIC" VALUE三"S(ARG BATTERY TOPIC)"/> <!:-- TOPIC NAME

OPIC)"/> <!-

TOPIC NAME

<PARAM NAME:"PUB SONAR" VALUE三"S(ARG PUB SONAL)"/> >::: PUB SONAR TOPIC OR NOT

PARAM NAME S

AM NAMER"SUD-ACKERMANN" VALUE三"S(ARG SUB ACKERMANN)"/> (-- SUB ACKERMANN TOPIC OR NOT

</NODE>

</GROUP>-->

<GROUP UNLESS"$(EVAL ROBOT NAME

group标签会将group里的内容划分成一个组，在launch中使用if unless作为判断语句。判断robot_name为空则执行第一个group中的内容，不为空则执行第二个group的内容。我们先来看robot_name为空时执行的第一个group
node标签：
node标签包括了节点的名称name、该节点所在的包名pkg，节点的可执行文件type以及输出方式output，“screen”表示输出在当前终端屏幕。
param标签：
节点中有param标签，它的作用也是声明参数。param和arg的区别在于，param声明的变量是节点所接收的，而arg的参数是launch文件所接收的。这里利用param标签设置了传入节点的参数，这些参数的值是通过value直接传入的固定值。例如：
<param name="baudrate" value="115200"/>
<param name="port" value='/dev/move_base'/>
也可以是通过value传入的一个参数
<param name="base_id" value="$(arg base_frame)"/>
这里base_id这个参数的值就是launch文件中arg标签声明的参数base_frame
robot_name不为空时执行的第二个group

GROUP UNLESS"$(EVAL ROBOT NAME

" TYPE-"BASE CONTROL.PY" OUTPUT-"SCREEN">

<NODE NAME 'BASE CONTROL*

PKGBASE CONTROL"

PARAM NAME BAUDRATE

VALUE115200/>

VALUE'/DEV/MOVE BASE'/>

<PARAM NAME " PORT"

VALUE-"$(ARG ROBOT NAME)/$(ARG BASE FRAME)"/> <!-- BASE LINK NAME

PARAM NAME BASE ID"

<!-. ODOM LINK NAME

PARAM NAME'ODOM ID"

VALUE-"$(ARG ROBOT NAME)/$(ARG ODOM FRAME)"/>

<!-- IMU LINK NAME

VALUE-"$(ARG ROBOT NAME)/$(ARG IMU FRAME)"/>

<PARAM NAME IMU ID"

<PARAM NAME:"ODOM TOPIC" VALUE-"S(ARG ODOM TOPIC)"/> <!---- TOPIC NAME

<PARAM NAME-"IMU TOPIC" VALUE-"$(ARG IMU TOPIC)"/> <!---- TOPIC NAME -

<PARAM NAME:"BATTERY TOPIC" VALUE-"S(ARG BATTERY TOPIC)"/> <!-- TOPIC NAME

984

<PARAM NAME3"PUB SONAR" VALUES"S(ARG PUB SONAR)"/> <(-- PUB SONAR TOPIC OR NOT --

<PARAM NAMES"SUB ACKERMANN" VALUES"S(ARG SUB ACKERMANN)"/> <1-- SUB ACKERMANN TOPIC OR NOT NOT

</NODE>

</GROUP>

</LAUNCH>

第二个group内又加了一个group标签，这个标签对应的是ns。ns是ros下的命名空间机制。使用命名空间后，会在话题名前加上robot_name这个变量所对应的参数值。以odom话题为例如果传入的robot_name是robot1，查看话题列表显示的就会是/robot1/odom。这样是为例便于支持多机器人，防止话题名重名。
script文件夹：
接下来打开script文件夹下的base_control.py文件
首先是载入用到的python包，读取环境变量。

SET AS INTERPRETER

#!/USR/BIN/PYTHON

# CODINGGBK

5 # DEVELOPER: FUZHI, LIU (LIU.FUZHIQWECHANGETECH.COM)

6 #

# LICENSED UNDER THE APACHE LICENSE, VERSION 2.0 (THE "LICENSE");

# YOU MAY NOT USE THIS FILE EXCEPT IN COMPLIANCE WITH THE LICENSE.

# YOU MAY OBTAIN A COPY OF THE LICENSE AT

HTTP://WWW.APACHE.ORG/LICENSES/LICENSE-2.0

11#

12 #

# UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING, SOFTWARE

# UNLES

# DISTRIBUTED UNDER THE LICENSE IS DISTRIBUTED ON AN "AS IS" BASIS,

# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.

# SEE THE LICENSE FOR THE SPECIFIC LANGUAGE GOVERNING PERMISSIONS AND

# LIMITATIONS UNDER THE LICENSE.

IMPORT OS

IMPORT ROSPY

IMPORT TF

IMPORT TIME

IMPORT SYS

IMPORT MATH

IMPORT

SERIAL

IMPORT STRING

FROM GEOMETRY MSGS.MSG IMPORT TWIST

FROM NAV MSGS.MSG IMPORT ODOMETRY

FROM SENSOR MSGS.MSG IMPORT BATTERYSTATE

FROM SENSOR

IMPORT IMU

MSGS.MSG

FROM SENSOR MSGS.MSG IMPORT RANGE

IMPORT CTYPES

BASE TYPE ; OS.GETENV('BASE TYPE')

IF OS.GETENV('SONAR NUM') IS NONE:

SONAR NUM O

ELSE:

SONAR_NUM - INT(OS.GETENV('SONAR_NUM'))

#CLASS QUEUE IS DESIGN FOR UART RECEIVE DATA CACHE

CLASS QUEUE:

base_control功能包支持多款机器人，BASE_TYPE这个环境变量用来区分不同型号的机器人。从系统的环境变量读取BASE_TYPE这个值，根据不同型号的机器人做出不同的判断。读取SONAR_NUM的值判断是否支持超声波，以及超声波的数量。
接下来是一个queue的类，用来实现环形队列的读出和存储

CLASS QUEUE:

INIT (SELF, CAPACITY 三 1024*4):

DEF

SELF.CAPACITY

CAPACITY

SELF.SIZE O

SELF.FRONT 0

SELF.REAR 0

SELF.ARRAY - [0]*CAPACITY

EMPTY(SELF):

DEF IS E

SELF.SIZE

RETURN O

DEF IS F

S FULL(SELF):

RETURN SELF.SIZE - SELF.CAPACITY

DEF ENQUEUE(SELF, ELEMENT):

IF SELF.IS FULL():

RAISE EXCEPTION('QUEUE IS FULL')

ELEMENT

SELF.ARRAY[SELF.REAR]

SELF.SIZE + 1

SELF.REAR 三 (SELF.REAR + 1) % SELF.CAPACITY

DEF DEQUEUE(SELF):

IF SELF.IS EMPTY():

RAISE EXCEPTION('QUEUE IS EMPTY')

SELF.SIZE -三 1

SELF.FRONT - (SELF.FRONT + 1) % SELF.CAPACITY

DEF GET FRONT(SELF):

RETURN SELF.ARRAY[SELF.FRONT]

DEF GET_FRONT SECOND(SELF):

RETURN SEL

RN SELF.ARRAY[(SELF.FRONT + 1) % SELF.CAPACITY)]

DEF GET QUEUE LENGTH(SELF):

AR - SELF.FRONT + SELF.CAPACITY) % SELF.CAPACITY

RETURN (SELF.REAR

DEF S

EF SHOW QUEUE(SELF):

FOR I IN RANGE(SELF.CAPACITY):

PRINT SELF.ARRAY[I],

PRINT('

#CLASS BASECONTROL IS DESIGN FOR HARDWARE BASE RELATIVE CONTROL

CLASS BASECONTROL:

接下来重点了解BaseControl这个类，它是实现底盘控制节点的关键内容

CLASS BASECONTROL:

INIT (SELF):

DEF

#GET PARAMS

BASE ID','BASE FOOTPRINT')

SELF.BASEID - ROSPY.GET_PARAM('~BASE_

SELF.ODOMID - ROSPY.GET PARAM('~ODOM ID','ODOM')

SELF.DEVICE PORT 三 ROSPY.GET PARAM('-PORT','/DEV/TTYUSBO')

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 RO

ROSPY.

Y.GET PARAM('~BATTERY_TOPIC','BATTERY')

: FLOAT(ROSPY.GET PARAM('~BATTERY FREQ','1'))

FREQ

SELF.BATTERY

LTOPIC ROSPY.GET PA

ET_PARAM('~CMD_VEL_TOPIC','/CMD VEL')

SELF.CMD_VEL_TO

SELF.ACKERMANN CMD TOPIC - ROSPY-GET PARAM('-ACKERMANN CMD TOPIC','/ACKERMANN (MD TOPIC')

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')

100

SELF.IMU FREQ - FLOAT(ROSPY.GET PARAM('-IMU FREQ','50'))

101

IF SELF.IMU_FREQ > 100:

102

SELF.IMU FREQ 100

103

104

SONAR - BOOL(ROSPY.GET PARAM('~PUB SOL

SONAR'',FALSE))

SELF

105

PUB

: BOOL(ROSPY.GET_PARAM('~SUB_ACKERMANN',FALSE))

SELF.SUB

ACKERMANN

106

107

108

#DEFINE PARAM

109

get params部分：获取各种参数，这里获取的参数就是launch文件中param标签设置的参数。如果没有在launch文件中获取到这些参数就会使用类中提前设置的默认值。
例如pub_imu这个参数，在launch文件和节点中默认使用的都是False只有在启动时传入true到launch文件，节点再接收launch文件中param标签设置的参数。判断pub_imu为true才会在节点中设置imu的变量信息。

#DEFINE PARAM

109

SELF.CURRENT_TIME - ROSPY.TIME.NOW()

110

SELF.PREVIOUS TIME - SELF.CURRENT TIME

111

SELF.POSEX00.0

112

SELF.POSE Y 0.0

113

SELF.POSE YAW 0.0

114

SELF.SERIALIDLE FLAG 三 0

115

SELF.TRANS X

<三0.0

116

SELF.TRANS_Y 0.0

117

SELF.ROTAT Z 三

118

三0.0

SELF.SPEED 0.0

119

SELF.STEERING ANGLE

120

E三0.0

SELF.SENDCOUNTER O

121

SELF.IMUERRFLAG - FALSE

122

SELF.ENCODERFLAG - FALSE

123

SELF.BATTERYFLAG - FALSE

124

SELF.ODOMTIMECOUNTER 三 O

125

BATTERYTIMECOUNTER 0

SELF

126

SELF.CIRCLELOOP

ELOOP QUEUE(CAPACITY 1024*4)

127

SELF.VX 0

128

SELF.VY 0

129

VYAW E O

SEL

130

YAWZ O

131

SEL

SELF.VVOLTAGE0

132

SELF.ICURRENT O

133

SELF.GYRO [0,0,0]

134

SELF.ACCELE[0,0,0]

135

SELF.QUAT [O,0,0,0]

136

SELF.SONAR 三 [0,0,0,0]

137

SELF.MOVEBASE FIRMWARE VERSION - [O,0]

138

SELF.MOVEBASE HARDWARE VERSION - [0,0]

139

,"NANOROBOT","4WD OMNI","4WD"]

SELF

140

SELF.MOVEBASE TYPE ['NANOCAR","NANO

37GB520"]

SELF.I

F.MOTOR TYPE ["25GA370"

141

TIME.NOW()

SELF.LAST CMD VEL TIME

ROSPY

142

SELF.LAST ACKERMANN CMD TIME

ROSPY.TIME.NOW()

143

define param：这里是BaseControl这个类用到的变量的定义。

# SERIAL COMMUNICATION

144

TRY:

145

SELF.SERIAL S

SERIAL.SERIAL(SELF.DEVICE PORT,SELF.BAUDRATE,TIMEOUT-10)

146

ROSPY.LOGINFO("OPENING SERIAL")

147

TRY:

148

IF SELF.SERIAL.IN WAITING:

149

SELF.SERIAL.READALL()

150

EXCEPT:

151

ROSPY.LOGINFO("OPENING SERIAL TRY FAILD")

152

153

PASS

154

EXCEPT:

ROSPY.LOGERR("CAN NOT OPEN SERIAL"+SELF.DEVICE PORT)

155

SELF.SERIAL.CLOSE

156

SYS.EXIT(O)

157

ROSPY.LOGINFO("SERIAL OPEN SUCCEED")

158

MIF MOVE BASE TYPE IS ACKERMANN CAR LLKE ROBOT AND USE ACKERMANN MSG ,SUD ACKERMANN TOPIC,ELSE SUD TO

159

串口通信部分：这部分的作用是打开机器人串口并连接到底盘上

#IF MOVE BASE TYPE IS ACKERMANN CAR LIKE ROBOT AND USE ACKERMANN MSA ,SUD A

ACKERMANN TOPIC,ELSE SUB

TOPIC

CMD VEL

159

IF(( NANOCAR' IN BASE TYPE) & (SELF.SUB ACKERMANN -- TRUE)):

160

FROM ACKERMANN MSGS.MSG IMPORT ACKERMANNDRIVESTAMPED

161

SELT-SUB - ROSDY,SUBSCRIBERJSELT-ACKERMANN CIND TOPLG,AGKERMANNDRIVESTANPED,SELT-ACKERMANDE SIZE3ZO3Z

162

ELSE:

163

SELF.SUB - ROSPY.SUBSCRIBER(SELF.CMD VEL TOPIC,TWIST,SELF.CMDCB,QUEUE SIZE-20)

164

这部分的内容包括不同机器人结构类型下订阅器的定义，当机器人型号为NanoCar并且使sub_ackermann == True将机器人设置为阿克曼结构类型（前轮转向后轮差速）。订阅器订阅消息类型为ackermann msg接收的话题为ackermann，一旦接收到话题就会进入ackermannCmdCB函数。否则订阅器订阅消息类型为Twist接收话题为cmd_vel，一旦收到话题就会进入cmdCB函数。

BARG

DEFAULT"FALSE"

NAME"SUB

ACKERMANN'

在launch文件中默认sub_ackermann为False
接下来是一些订阅器和发布器的定义包括里程计的发布器，话题消息的发布。如果机器人支持超声波还会定义一些超声波消息类型的发布。此外还包括tf坐标发布器实时发布坐标偏移量，设置里程计和话题消息的发布频率需要的定时器，与底盘通讯的定时器。

.CMDCB,QUEUE SIZE-20)

SELF.SUB

: ROSPY.SUBSCRIBER(SELF.CMD VEL TOPIC,TWIST,SELF

164

SELF.PUB - ROSPY.PUBLISHER(SELF.ODOM TOPIC,ODOMETRY,QUEUE SIZE-10)

165

- ROSPY.PUBLISHER(SELF.BATTERY TOPIC,BATTERYSTATE,QUEUE SIZE-3)

SELF

LF.BATTERY PUB

166

IF SELF.PUB SONAR:

167

IF SONAR NUM >

>0:

168

F.TIMER SONAR - ROSPY.TIMER(ROSPY.DURATION(100.0/1000),SELF.TIMERSONARCB)

SELF.TIN

169

SELF.RANGE PUB1 - ROSPY.PUBLISHER('SONAR 1',RANGE,QUEUE SIZE-3)

170

IF SONAR NUM > 1:

171

三 ROSPY.PUBLISHER('SONAR 2',RANGE,QUEUE SIZE-3)

SELF.RANGE PUB2

172

SONAR NUM > 2:

173

.PUBLISHER('SONAR 3',RANGE,QUEUE SIZE-3)

SELF. RANGE PUB3 ROSPY.

174

SONAR NUM>3:

175

SELF.RANGE PUB4 - ROSPY.PUBLISHER('SONAR 4',RANGE,QUEUE SIZE-3)

176

177

SELF.TF BROADCASTER 三 TF.TRANSFORMBROADCASTER()

178

DOM FREQ),SELF.TIMERODOMCB)

SELF.TIMER ODOM - ROSPY.TIMER(ROSPY.DURATION(1.0/SELF.ODOM FREG)

179

SELF,TIMER BATTERY 三 ROSPY.TIMER(ROSPY,DURATION(1.8/SELF.BATTERY FREQ),SELF.TIMERBATTERYCB)

180

SELF.TIMER COMMUNICATION - ROSPY.TIMER(ROSPY.DURATION(1.0)

0/500),SELF.TIMERCOMMUNICATIONCB)

181

再往下是imu的发布器和定时器的定义，getVersion()函数的作用是获取底盘软硬件版本号

#INORDER TO COMPATIBILITY OLD VERSION FIRMWARE,INU TOPIC IS NOT PUD IN DEFAULT

183

IF(SELF.PUB IMU):

184

SELF.TIMER IMU - ROSPY.TIMER(ROSPY.DURATION(1.0/SELF.INU FREQ),SELF.TIMERIMUCB)

185

SELF.IMU PUB - ROSPY.PUBLISHER(SELF.IMU TOPIC,IMU,QUEUE SIZE-10)

186

187

SELF.GETVERSION()

在redme文件中有定义这些通讯协议的格式以及内容

6,线速度单位为M/S,角速度单位为RAD/S(孤度制),角度单位为度(角度制)

功能码

数据

预留位CRC校验

帧头

帧长度

RXXX

OXXX...OXXX

0X01

0X00

0X5A

0X00

帧头:

1BYTE0X5A固定值

帧长度:

赖头(1 BYTE)+U长度(1 BYTE)+JD(1 BYTE)+功能码(1 BYTE)+数据(8-250BYTE)+数据(1 BYTE)+CRC权验(1 BYTE)+CRC权验(1 BYTE)

功能码:

1BYTE控制板端发送的功能码为偶数,控制板端接收的功能码为奇数

数据:

长度和内容具体参照各功能码定义

预留位:

目前设置为0X00,为将来协议可能的扩展预留

CRC校验

1BYTE,校验方式为CRC-8/MAXIM,设置为0XFF,则强制不进行CRC校验

(参考在线计算网站:HTTP://WWW.IP33.COM/CRC.HTML)

例如获取底盘软硬件版本号的协议。

MOTOR TYPE

RAT

BASE TYPE

131

CPI4INITSETUP.SH

132

SETBASE.SH

粤XF1

133

CMAKELISTS.TXT

上位机向下位机查询版本号

134

PACKAGE.XML

例:5A0601F100D7

135

README.MD

136

BINGDA_PRACTICES

0XF2

137

下位机回复版本号,数据长度为6BYT

BINGDA TUTORIALS

138

BYTE1

BYTE2 BYTE3

BYT

139

LIDAR

140

ROBOT NAVIGATION

141

ROBOT SIMULATION

0XF3

142

ROBOT VISION

上位机向下位机查询主板SN号

143

11.ZIP

例:5AQ601F3046

1AA

接下来是一个延迟函数，获取通讯协议与地盘建立连接后会初始化imu，在此期间不接收数据所以需要这个延迟。getSN()函数的作用是查询主板SN号，getInfo()获取底盘配置信息

SELF.GETVERSION()

187

#MOVE BASE IMU INITIALIZATION NEED ABOUT 2S,DURIR

188

#SO NEED THIS GAP

189

TIME.SLEEP(2.2)

190

SELF.GETSN()

191

TIME.SLEEP(0.01)

192

SELF.GETINFO()

193

194

#CRC-8 CALCULATE

接下来是定义了crc计算函数用来做通讯校验

#CRC-8 CALCULATE

194

DEF CRC LBYTE(SELF,DATA):

195

CRC IBYTE O

196

FOR I IN RANGE(0,8):

197

IF((CRC LBYTE DATA)&0X01):

198

CRC 1BYTE-0X18

199

LBYTE>>三1

200

CRC

CRC 1BYTE0X80

201

ELSE:

202

CRC 1BYTE>>1

203

DATA>>-1

204

IBYTE

RETURN CRC

205

DEF CRC BYTE(SELF,DATA,LENGTH):

206

207

RET O

FOR I IN RANGE(LENGTH):

208

RET : SEIF.CRC 1BYTE(RET(DATA[IL)

209

RETURN

RET

210

cmdCB()函数，cmdCB会把接收到的话题的关键信息取出来，然后根据通信协议做一个转换。再通过串口发送到底盘上，实现对底盘的控制，

DEF

CMDCB(SELF,DATA):

212

SELF.TRANS_X - DATA.LINEAR.X

213

SELF.TRANS Y - DATA.LINEAR.Y

214

SELF.ROTAT Z DATA.ANGULAR.Z

215

SELF.LAST CMD VEL TIME ROSPY.TIME.NOW()

216

OUTPUT - CHR(0X5A) + CHR(12) + CHR(0X01) + CHR(0X01)

217

CHR((INT(SELF.TRANS X*1000.0)>8)SEXFF)+ CHR(INT(SELF.TRANS X*1009.0)SOXFF)

218

CHR((INT(SELF.TRANS_Y*1000.0)>>8)60XFF) + CH

CHR(INT(SELF.TRANS Y*1000.0)G0XFF)

219

CHR((INT(SELF.ROTAT Z*1000.0)>>8)&0XFF) +

+ CHR(INT(SELF.ROTAT 2*1000.0)G0XFF)

220

CHR(OX00)

221

OUTPUTDATA - [EX5A,OXEC,0X01,OXE1,OX00,EX00,EX00,EX00,EX00,EX00,EX00,0X00]

222

OUTPUTDATA[4] 三 (INT(SELF.TRANS X*1000.0)>8)SOXFF

223

OUTPUTDATA[5] - INT(SELF.TRANS X*1000.0)GOXFF

224

OUTPUTDATA[6] - (INT(SELF.TRANS Y*1000.0)>>8)GOXFF

225

OUTPUTDATA[7] - INT(SELF.TRANS Y*1000.0)GOXFF

226

OUTPUTDATA[8] - (INT(SELF.ROTAT 2*1000.0)>8)60XFF

227

OUTPUTDATA[9] - INT(SELF.ROTAT 2*1000.0)GOXFF

228

CRC 8 - SELF.CRC BYTE(OUTPUTDATA,LEN(OUTPUTDATA)-1)

229

OUTPUT + CHR(CRC8)

230

WHILE SELF.SERIALIDLE FLAG:

231

TIME.SLEEP(O.01)

232

SELF.SERIALIDLE FLAG

233

TRY:

234

WHILE SELF.SERIAL.OUT WAITING:

235

236

PASS

SELF.SERIAL.WRITE(OUTPUT)

237

EXCEPT:

238

ROSPY.LOGERR("VEL COMMAND SEND FAILD")

239

SELF.SERIALIDLEFLAG

240

timerCommunicationCB函数，通讯定时器回调函数。当定时时间到，检查串口中是否有缓存的数据。有则读取，存放到环形消息队列中。

PROTOCOL

#DEPEND ON COMMUNI CATION

DEF

TIMERCOMMUNICATIONCB(SELF,EVENT):

LENGTH - SELF.S

LF.SERIAL.IN WAITING

IF LENGTH:

READING 三 SELF.SERIAL.READ ALL()

IFLEN(READING)!0:

FOR I IN RANGE(0,LEN(READING)):

DATA - (INT(READING[I].ENCODE('HEX'),16))

TRY:

SELF.CIRCLELOOP.ENQUEUE(DATA)

EXCEPT:

PASS

ELSE:

将环形队列中的内容重新读出来，如果读出来的数据是数据帧的帧头就获取数据长度，做crc校验。并根据功能码设置读取不同的值。所有底盘从串口发送的数据都是在这个回调函数里做的解析处理

IF SELF.CIRCLELOOP.IS EMPTY()--FALSE:

283

DATA 三 SELF.CIRCLELOOP.GET FRONT()

284

IF DATA OX5A:

285

LENGTH

SELF.CIRCLELOOP.GET FRONT SECOND()

286

IF LENGTH > 1 :

287

F SELF.CIRCLELOOP.GET FRONT SECOND() <+ SELF.CIRCLELOOP.GET QUEUE LENGTH();

IF S

288

DATABUF []

289

FOR I IN RANGE(LENGTH):

290

DATABUF.APPEND(SELF.CIRCLELOOP.GET FRONT())

291

SELF.CIRCLELOOP.DEQUEUE()

292

293

IF (DATABUF(LENGTH-11) 三 SELF.CRC BYTE(DATABUF,LENGTH-1):

294

295

PASS

ELSE:

296

297

PASS

298

#PARSE RECEIVE DATA

IF(DATABUF[3] 0X04):

299

DATABUF[4]*256

SELF.VX

X 三

300

DATABUF[5]

SELF.VX

+二

301

DATABUF[6]*256

SELF.VY

302

DATABUF[7]

SELF.VY

303

十川

DATABUF[8]*256

SELF.VYAW

304

F.VYAW+DATABUF[9]

SELI

305

ELIF(DATABUF[3] - 0X06):

306

SELF.YAWZ 三 DATABUF[8]*256

307

SELF.YAWZ+DATABUF[9]

308

ELIF (DATABUF[3] - 0X08):

309

timerOdomCB函数，通过python使用串口给底盘发送一条消息来获取里程计信息。

DEF TIMERODOMCB(SELF,EVENT):

#GET MOVE BASE VELOCITY DATA

FIRMWARE VERSION[1] 0:

IF SELF.MOVEBASE

#OLD VERSION FIRMWARE HAVE NO VERSION INFO AND NOT SUPPORT NEW COMMAND BELOW

OUTPUT - CHR(OX5A)+ CHR(EX3O) + CHT(OXO1)+ CHT(OX99) + CHR(OXO)+ CHR(OX3O) + CHR(OX38 IS CRC-B VALUE

ELSE:

#IN FIRMWARE VERSION NEW THAN VL.1.0,SUPPORT THIS COMMAND

OUTPUT - CHR(OX5A)+ CHR(OX06)+ CHR(0X01)+ CHR(OX11)+ CHR(OX00)+ CHR(OXA2)

WHILE(SELF.SERIALIDLE_FLAG):

TIME.SLEEP(0.01)

SELF.SERIALIDLE FLAG 1

TRY:

WHILE SELF.SERIAL.OUT_WAITING:

PASS

SELF.SERIAL.WRITE(OUTPUT)

EXCEPT:

ROSPY.LOGERR("ODOM COMMAND SEND FAILD")

SELF.SERIALIDLE FLAG 0

底盘返回的里程计信息在timerCommunicationCB函数中做解析后取得速度赋值给局部变量，再通过速度对时间做积分得出相对位移。然后定义了一个里程计的消息类型，将前面计算出来的值赋予到消息中。最后通过里程计发布器发布出去。

441

#CALCULATE ODOM DATA

VX - FLOAT(CTYPES.C INT16(SELF.VX).VALUE/1000.0)

442

VY - FLOAT(CTYPES.C.INT16(SELF.VY).VALUE/1000.0)

443

VYAW - FLOAT(CTYPES.C INT16(SELF.VYAW).VALUE/1000.0)

444

445

SELF.POSE YAW - FLOAT(CTYPES.C INT16(SELF.YAWZ).VALUE/100.0)

446

SELF.POSE YAW - SELF.POSE YAW*MATH.PI/180.0

447

448

SELF.CURRENT TIME ROSPY.TIME.NOW()

449

DT - (SELF.CURRENT TIME - SELF.PREVIOUS TIME).TO SEC()

450

SELF.PREVIOUS TIME SELF.CURRENT TIME

451

SELF.POSE X - SELF.POSE X + VX * (MATH.COS(SELF.POSE YAW))*DT - VY

(MATH.SIN(SELF.POSE YAW))*DT

452

SELF.POSE_Y - SELF.POSE_Y + VX * (MATL

(MATH.COS(SELF.POSE YAW))*DT

(MATH.SIN(SELF.POSE YAW))*DT+VY

453

454

POSE QUAT - TF.TRANSFORMATIONS.QUATERNION FROM EULER(0,SELF.POSE YAW)

455

MSG ODOMETRY()

456

MSG.HEADER.STAMP SELF.CURRENT TIME

457

MSG.HEADER.FRAME ID SELF.ODOMID

458

MSG.CHILD FRAME ID SELF.BASEID

459

MSG.POSE.POSE.POSITIONXX S

SELFPOSEX

460

SELF.POSE_Y

MSG.POSE.POSE.POSITION.Y

461

MSG.POSE.POSE.POSITION.Z

462

MSG.POSE.POSE.ORIENTATION.X 三 POSE QUAT[0]

463

MSG.POSE.POSE.ORIENTATION.Y 三 POSE QUAT[1]

464

MSG.POSE.POSE.ORIENTATION.Z - POSE QUAT[2]

465

466

MSG.POSE.POSE.ORIENTATION.W - POSE_QUAT[3]

MSG.TWIST.TWIST.LINEAR.X

467

MSG.TWIST.TWIST.LINEAR.Y

468

MSG.TWIST.TWIST.ANGULAR.Z VYAW

469

SELF.PUB.PUBLISH(MSG)

470

SALT,TT BROADTASTER,SENDTRANSTORM( SELT,BOSE X,SETT,SELT,SELT,SELT,BOSE QUAT,SELT,SELT,BASERD,SOTT.

471

#BATTERY TIMER CALLBACK FUNCTION TO GET BATTERY INFO

472

timerBatteryCB函数，给底盘发送查询电池信息的指令，再将获取到的数值传输到电池类型的消息中，最后通过发布器发布出去。

DEF TIMERBATTERYCB(SELF,EVENT):

473

OUTPUT 三 CHR(0X5A) + CHR(0X06)

CHR(0X00) + CHR(0XE4) #0XE4 IS CRC-8 VALUE

CHR(0X01)+ CHR(0X07)

474

WHILE(SELF.SERIALIDLE FLAG):

475

SELF.SERIALIDLE FLAG

477

478

TRY:

EXCEPT:

482

SELF.SERIALIDLE FLAG O

484

MSG BATTERYSTATE()

485

MSG.HEADER.STAMP - SELF.CURRENT TIME

486

MSG.HEADER.FRAME ID - SELF.BASEID

487

MSG.VOLTAGE FLOAT(SELF.VVOLTAGE/1000.0)

488

MSG.CURRENT FLOAT(SELF.ICURRENT/1000.0)

489

SELF.BATTERY_PUB.PUBLISH(MSG)

490

#SONAR TIMER CALLBACK FUNCTION TO GET BATTERY INFO

491

再往后就是timerSonarCB和timerIMUCB，这两个回调函数和前面的几个回调函数实现原理一致，都是先给底盘发送指令，底盘传回的数据再经由timerCommunicationCB函数解析处理，回调函数再将获取到的值发布出去。
以上就是cript文件夹下base_control.py文件的大致内容和文件结构，接下来我们看一下cript文件夹下的其他几个文件
setbase.sh：
#!/bin/bash
BASE_TYPE=$1
echo "export BASE_TYPE=$BASE_TYPE" >> ~/.bashrc
source ~/.bashrc
这个脚本文件的作用是定义机器人底盘类型，它将执行脚本后输入的一个变量作为环境变量写入~/.bashrc文件

BINGDAQUBUNTU:~/CATKIN WS/SRC/BASE.CONTROL/SCRIPTS

RIPTS /SETBASE.SH 4WD

BINGDAQUBUNTU:~/CATKIN_WS/SRC/BASE_CONTROL/SCRIPTS

以4WD为例，设置完以后在原有的基础上多了一行

EXPORT BASE TYPENOCAR

EXPORT LIDAR TYPERPLIDAR

EXPORT CAMERA_TYPEASTRAPRO

EXPORT SONAR NUM2

EXPORT BASE TYPE4WD

此时source使.bashrc生效后的BASE_TYPE这个环境变量会等于4WD，这是因为出现同名的环境变量时只生效文件最末尾的。

BINGDAQUBUNTU:-/CATKIN-WS/SRC/BASE_CONTROL/SCRIPTS SOURCE -/ BASHRC

BINGDAQUBUNTU:~/CATKIN_WS/SRC/BASE_CONTROL/SCRIPTS

ECHO $BASE TYPE

4WD

BINGDAQUBUNTU:~/CATKIN WS/SRC/BASE_CONTROL/SCRIPT$

rpi4initsetup.sh：
#!/bin/bash
echo 'KERNEL=="ttyS0", MODE:="0666", GROUP:="dialout", SYMLINK+="move_base"' >/etc/udev/rules.d/move_base_pi4.rules
service udev reload
sleep 1
service udev restart
这个文件的作用是，端口udev规则转换。树莓派与机器人连接使用的是硬件串口其端口名为ttyS0这个端口名无法修改，这个脚本会将ttyS0创建一个符号链接，move_base链接到ttyS0并赋予它所有用户可读可写权限。
使用udev规则的另一个好处就是可以统一端口名称，当主机与机器人底盘使用USB转串口的方式连接时端口名是不确定的。以4WD为例
initsetup.sh：
#!/bin/bash
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", GROUP:="dialout", SYMLINK+="move_base"' >/etc/udev/rules.d/move_base_340.rules
service udev reload
sleep 1
service udev restart
4WD使用的是USB转串口的芯片，它的端口名有可能是ttyUSB0也有可能是ttyUSB1无法确定具体名称。当其他使用USB转串口芯片的设别也接入主机时，很难分辨各个端口名所对应的设备。所以我们给底盘使用的USB转串口的芯片设定了一个规则，根据芯片的Vender ID 和 Product ID进行筛选。

好好学习28

关注

16
点赞
踩
12

收藏

觉得还不错? 一键收藏
1
评论
6.1base_control功能包代码解析

ns是ros下的命名空间机制。arg标签是用来在launch文件中声明参数的，在launch中声明过的参数都可以在启动launch文件时传入已声明过的参数。param和arg的区别在于，param声明的变量是节点所接收的，而arg的参数是launch文件所接收的。例如pub_imu这个参数，在launch文件和节点中默认使用的都是False只有在启动时传入true到launch文件，节点再接收launch文件中param标签设置的参数。接下来是一些订阅器和发布器的定义包括里程计的发布器，话题消息的发布。
复制链接

扫一扫