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
<PACKAGE>
<NAME>BASE CONTROL</NAME>
<VERSION>1.3.0</VERSION>
<DESCRIPTION>THE BASE CONTROL PACKAGE</DESCRIPTION>
ONE MAINTAINER TAG REQUIRED, MULTIPLE ALLOWED, ONE PERSON PER TAG
8
EXAMPLE:
9
<MAINTAINER EMAIL-"JANE.DOEQEXAMPLE.COM">JANE DOE</MAINTAINER>
<MAINTAINER EMAIL三"LIU.FUZHIQWECHANGETECH.COM">FUZHI, LIU</MAINTAINER>
10
11
12
ONE LICENSE TAG REQUIRED, MULTIPLE ALLOWED, ONE LICENSE PER TAG -
13
<!-- COMMONLY USED LICENSE STRINGS: -->
14
BSD, MIT, BOOST SOFTWARE LICENSE, GPLV2, GPLV3, LGPLV2.1, LGPLV3
<!--
15
<LICENSE>APACHE 2.0</LICENSE>
16
文件末尾还包含一些run_depend标签,它是用来帮助rosdep这个工具解决依赖项的问题
DEPEND>ROSPY</RUN DEPEND>
<RUN
DEPEND>TF</RUN_DEPEND>
44
<RUN
DEPEND>GEOMETRY MSGS</RUN DEPEND>
<RUN
45
DEPEND>NAV_MSGS</RUN
DEPEND>
<RUN
46
_DEPEND>SENSOR_MSGS</RUN_DEPEND>
47
<RUN
DEPEND>ACKERMANN MSGS</RUN
DEPEND>
48
<RUN
_DEPEND>TELEOP_TWIST_KEYBOARD</RUN_DEPEND>
49
<RUN
50
51
<BUILDTOOL
_DEPEND>CATKIN</BUILDTOOL DEPEND>
launch文件夹:
打开launch文件夹,它下面只有一个base_control.launch文件
BASE_CONTROL.LAUNCH
SRC > BASE_CONTROL > LAUNCH >
<?XML VERSION"1.0"?>
1
2
BLAUNCHB
3
45678
<!- SUPPORT MULTI ROBOT
<ARG NAME"ROBOT NAME" DEFAULT""/>
SUPPORT MULTI ROBOT
<!- ROBOT FRAME
DEFAULT-"BASE FOOTPRINT" />
NAME"BASE FRAME"
<ARG
9
DEFAULT"ODOM"/>
<ARG NAME:"ODOM_FRAME"
DEFAULT"IMU" />
'IMU FRAME"
10
<ARG
NAME
TOPIC
11
PUD
DEFAULT-"ODOM" /><!-- DO NOT USE '/' IN TOPIC 4
12
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标签:
<XML VERSION-"1.0"?>
1234567
<LAUNCH>
<!- SUPPORT MULTI ROBOT
<ARG NAME"ROBOT_NAME" DEFAULT-""/>
SUPPORT MULTI ROBOT
<!- ROBOT FRAME
DEFAULT-"BASE FOOTPRINT" />
<ARG NAME "BASE_FRAME"
DEFAULT"ODOM" />
NAMEODOMFRAME
<ARG N
9
DEFAULT"IMU" />
<ARG NAME"IMU_FRAME"
10
<!-- PUB TOPIC -->
11
DEFAULT-"ODOM" /><!-- DO NOT USE '/' IN TOPIC NAME-->
<ARG NAME"ODOM TOPIC"
12
DEFAULT"IMU"/>
<ARG NAME"IMU TOPIC"
13
DEFAULT"BATTERY"/>
<ARG NAME"BATTERY TOPIC"
14
<!-- SUB TOPIC
15
<ARG NAME"CMD_VEL_TOPIC"
DEFAULT-"CMD VEL" /> <!-- DO NOT USE '/" IN TOPIC NAME-
16
DEFAULT:"ACKERMANN_CMD" /2
GARG NAME-"ACKERMANN_CMD_TOPIC"
17
<!- CONFIG PARAM
18
DEFAULT FALSE"/>
<ARG NAME"PUB IMU"
19
DEFAULT "FALSE" />
<ARG NAME"SUB ACKERMANN"
20
DEFAULT"FALSE" />
21
<ARG NAME "PUB SONAR"
L NODE
<!-BASE CONTROL R
22
23
<!--ROS NAMESPACE NAME CAN NOT BE EMPTY,SO NEED EVALUATE USE USE MULTI ROBOT OR NOT--7
24
25
<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
24
SO HEED EVALUATE USE MULTI ROBOT OR HOT-->
25
<GROUP IF-'$(EVAL ROBOT NAME
PKG-"BASE CONTROL" TYPE-"BASE CONTROL.PY" OUTPUT:"SCREEN""
26
<NODE NAME BASE CONTROL"
27
PARAM NAME BAUDRATE
VALUE115200"/>
VALUE-'/DEV/MOVE BASE'/>
28
PARAM NAME-"PORT"
29
VALUE-"$(ARG BASE FRAME)"/> <!-- BASE LINK NAME
30
<PARAM NAME BASE ID"
<!-- ODOM LINK NAME
VALUE"$(ARG ODOM_FRAME)"/>
31
PARAM NAME "ODOM ID"
<!-- IMU LINK NAME
32
VALUE-"$(ARG IMU_FRAME)"/>
<PARAM NAME "IMU ID"
33
<PARAM NAME三"ODOM TOPIC" VALUE-"S(ARG ODOM TOPIC)"/> <!---- TOPIC NAME
34
35
<PARAM NAME:"IMU TOPIC" VALUE三"S(ARG IMU TOPIC)"/> <!--- TOPIC NAME
36
<PARAM NAME三"DATTERY TOPIC" VALUE三"S(ARG BATTERY TOPIC)"/> <!:-- TOPIC NAME
37
38
<PARAM NAME-"CMD VEL_TOPIC" VALUE-"$(ARG CMD VEL_TOPIC)"/>
OPIC)"/> <!-
TOPIC NAME
39
<PARAM NAME三"ACKERMANN_CMD TOPIC" VALUE-"S(ARG ACKERMANN_CMD TOPIC)"/>
40
41
42
<PARAM NAME:"PUB SONAR" VALUE三"S(ARG PUB SONAL)"/> >::: PUB SONAR TOPIC OR NOT
43
44
PARAM NAME S
AM NAMER"SUD-ACKERMANN" VALUE三"S(ARG SUB ACKERMANN)"/> (-- SUB ACKERMANN TOPIC OR NOT
</NODE>
45
46
</GROUP>-->
47
48
<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
48
<GROUP NS-"$(ARG ROBOT NAME)">
49
58
" 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 -
60
<PARAM NAME:"BATTERY TOPIC" VALUE-"S(ARG BATTERY TOPIC)"/> <!-- TOPIC NAME
984
<PARAM NAME-"CMD VEL TOPIC" VALUE-"$(ARG CMD VEL TOPIC)"/>
65
66
<PARAM NAME3"PUB SONAR" VALUES"S(ARG PUB SONAR)"/> <(-- PUB SONAR TOPIC OR NOT --
67
<PARAM NAMES"SUB ACKERMANN" VALUES"S(ARG SUB ACKERMANN)"/> <1-- SUB ACKERMANN TOPIC OR NOT NOT
</NODE>
68
</GROUP>
69
</GROUP>
70
71
72
</LAUNCH>
第二个group内又加了一个group标签,这个标签对应的是ns。ns是ros下的命名空间机制。使用命名空间后,会在话题名前加上robot_name这个变量所对应的参数值。以odom话题为例如果传入的robot_name是robot1,查看话题列表显示的就会是/robot1/odom。这样是为例便于支持多机器人,防止话题名重名。
script文件夹:
接下来打开script文件夹下的base_control.py文件
首先是载入用到的python包,读取环境变量。
SET AS INTERPRETER
1
#!/USR/BIN/PYTHON
2
# CODINGGBK
3
# COPYRIGHT 2019 WECHANGE TECH.
4
5 # DEVELOPER: FUZHI, LIU (LIU.FUZHIQWECHANGETECH.COM)
6 #
# LICENSED UNDER THE APACHE LICENSE, VERSION 2.0 (THE "LICENSE");
7
# YOU MAY NOT USE THIS FILE EXCEPT IN COMPLIANCE WITH THE LICENSE.
8
# YOU MAY OBTAIN A COPY OF THE LICENSE AT
9
#
10
HTTP://WWW.APACHE.ORG/LICENSES/LICENSE-2.0
11#
12 #
# UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING, SOFTWARE
# UNLES
13
# DISTRIBUTED UNDER THE LICENSE IS DISTRIBUTED ON AN "AS IS" BASIS,
14
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
15
# SEE THE LICENSE FOR THE SPECIFIC LANGUAGE GOVERNING PERMISSIONS AND
16
# LIMITATIONS UNDER THE LICENSE.
17
IMPORT OS
18
IMPORT ROSPY
19
IMPORT TF
20
IMPORT TIME
21
IMPORT SYS
22
IMPORT MATH
23
IMPORT
SERIAL
24
IMPORT STRING
25
FROM GEOMETRY MSGS.MSG IMPORT TWIST
26
FROM NAV MSGS.MSG IMPORT ODOMETRY
27
FROM SENSOR MSGS.MSG IMPORT BATTERYSTATE
28
FROM SENSOR
IMPORT IMU
29
MSGS.MSG
FROM SENSOR MSGS.MSG IMPORT RANGE
30
31
IMPORT CTYPES
32
33
BASE TYPE ; OS.GETENV('BASE TYPE')
34
IF OS.GETENV('SONAR NUM') IS NONE:
35
SONAR NUM O
36
ELSE:
37
SONAR_NUM - INT(OS.GETENV('SONAR_NUM'))
38
39
40
#CLASS QUEUE IS DESIGN FOR UART RECEIVE DATA CACHE
CLASS QUEUE:
base_control功能包支持多款机器人,BASE_TYPE这个环境变量用来区分不同型号的机器人。从系统的环境变量读取BASE_TYPE这个值,根据不同型号的机器人做出不同的判断。读取SONAR_NUM的值判断是否支持超声波,以及超声波的数量。
接下来是一个queue的类,用来实现环形队列的读出和存储
CLASS QUEUE:
42
INIT (SELF, CAPACITY 三 1024*4):
DEF
43
SELF.CAPACITY
CAPACITY
44
SELF.SIZE O
45
SELF.FRONT 0
46
SELF.REAR 0
47
SELF.ARRAY - [0]*CAPACITY
48
49
EMPTY(SELF):
DEF IS E
50
SELF.SIZE
RETURN O
51
52
DEF IS F
S FULL(SELF):
53
RETURN SELF.SIZE - SELF.CAPACITY
54
55
DEF ENQUEUE(SELF, ELEMENT):
56
IF SELF.IS FULL():
57
RAISE EXCEPTION('QUEUE IS FULL')
58
ELEMENT
59
SELF.ARRAY[SELF.REAR]
60
SELF.SIZE + 1
SELF.REAR 三 (SELF.REAR + 1) % SELF.CAPACITY
61
62
DEF DEQUEUE(SELF):
63
IF SELF.IS EMPTY():
64
RAISE EXCEPTION('QUEUE IS EMPTY')
65
SELF.SIZE -三 1
99
SELF.FRONT - (SELF.FRONT + 1) % SELF.CAPACITY
67
68
DEF GET FRONT(SELF):
69
RETURN SELF.ARRAY[SELF.FRONT]
70
71
DEF GET_FRONT SECOND(SELF):
72
RETURN SEL
73
RN SELF.ARRAY[(SELF.FRONT + 1) % SELF.CAPACITY)]
74
DEF GET QUEUE LENGTH(SELF):
75
AR - SELF.FRONT + SELF.CAPACITY) % SELF.CAPACITY
76
RETURN (SELF.REAR
77
DEF S
EF SHOW QUEUE(SELF):
78
FOR I IN RANGE(SELF.CAPACITY):
79
PRINT SELF.ARRAY[I],
80
PRINT('
81
#CLASS BASECONTROL IS DESIGN FOR HARDWARE BASE RELATIVE CONTROL
82
CLASS BASECONTROL:
83
接下来重点了解BaseControl这个类,它是实现底盘控制节点的关键内容
CLASS BASECONTROL:
83
INIT (SELF):
DEF
84
#GET PARAMS
85
BASE ID','BASE FOOTPRINT')
SELF.BASEID - ROSPY.GET_PARAM('~BASE_
86
SELF.ODOMID - ROSPY.GET PARAM('~ODOM ID','ODOM')
87
SELF.DEVICE PORT 三 ROSPY.GET PARAM('-PORT','/DEV/TTYUSBO')
88
SELF.BAUDRATE - INT(ROSPY GET PARAM('-BAUDRATE','115200')
89
SELF.ODOM FREQ - INT(ROSPY.GET PARAM('~ODOM FREQ','50'))
90
SELF.ODOM TOPIC ROSPY.GET
PARAM('~ODOM TOPIC','/ODOM' )
91
SELF.BATTERY TOPIC RO
ROSPY.
Y.GET PARAM('~BATTERY_TOPIC','BATTERY')
92
: FLOAT(ROSPY.GET PARAM('~BATTERY FREQ','1'))
FREQ
SELF.BATTERY
93
LTOPIC ROSPY.GET PA
ET_PARAM('~CMD_VEL_TOPIC','/CMD VEL')
SELF.CMD_VEL_TO
94
SELF.ACKERMANN CMD TOPIC - ROSPY-GET PARAM('-ACKERMANN CMD TOPIC','/ACKERMANN (MD TOPIC')
95
96
SELF.PUB IMU - BOOL(ROSPY.GET PARAM('~PUB IMU',FALSE))
97
IF(SELF.PUB IMU TRUE):
98
SELF.IMUID - ROSPY.GET_PARAM('~IMU_ID','IMU')
99
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
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'
20
在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
IF
SONAR NUM > 2:
173
.PUBLISHER('SONAR 3',RANGE,QUEUE SIZE-3)
SELF. RANGE PUB3 ROSPY.
174
IF
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(孤度制),角度单位为度(角度制)
14
15
功能码
数据
预留位CRC校验
帧头
帧长度
ID
16
RXXX
OXXX...OXXX
0X01
0X00
0X00
0X5A
0X00
17
18
帧头:
19
1BYTE0X5A固定值
20
帧长度:
21
赖头(1 BYTE)+U长度(1 BYTE)+JD(1 BYTE)+功能码(1 BYTE)+数据(8-250BYTE)+数据(1 BYTE)+CRC权验(1 BYTE)+CRC权验(1 BYTE)
22
功能码:
23
1BYTE控制板端发送的功能码为偶数,控制板端接收的功能码为奇数
24
数据:
25
长度和内容具体参照各功能码定义
26
预留位:
27
目前设置为0X00,为将来协议可能的扩展预留
28
CRC校验
29
1BYTE,校验方式为CRC-8/MAXIM,设置为0XFF,则强制不进行CRC校验
30
(参考在线计算网站:HTTP://WWW.IP33.COM/CRC.HTML)
31
例如获取底盘软硬件版本号的协议。
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
ZZ
YY
AA
XX
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
:3
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进行筛选。