6、冰达ROS代码解读

最新推荐文章于 2024-08-09 23:06:46 发布

好好学习28

最新推荐文章于 2024-08-09 23:06:46 发布

阅读量798

点赞数 9

文章标签：人工智能计算机视觉

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

版权

6.1base_control功能包代码解析

本节内容主要讲解base_control功能包的代码内容和各个部分的作用

虚拟机中打开Visu

6.1base_control功能包代码解析

6.2robot_vision功能包代码解析

6.3雷达文件夹和robot_navigation功能包代码解析

lidar.launch的作用是，根据读取机器人底盘类型和雷达类型，做启动节点和TF坐标转换的判断。在lidar.launch中又根据雷达类型不同包含了不同类型的launch。这些launch都存放在功能包下launch文件夹下的lidar文件夹中。

6.4robot_simulation功能包代码解析

al Studio Code点击file->Open Folder打开工作空间，工作空间下找到src下的base_control功能包。

CMakeLists.txt文件：

CMakeLists.txt是创建功能包时自动产生的文件。由于base_control这个功能包的源码使用的python语言编写，也没有使用自定义消息或者服务类型，对这个文件的内容没有修改。

package.xml文件：

package.xml用来做功能包的描述，它内容包含有功能包名，版本号，功能包功能描述，作者联系邮箱，开源协议等等。

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

launch文件夹：

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

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

launch标签：

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

arg标签：

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

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

roslaunch base_control base_control.launch pub_imu:=ture

group标签：

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直接传入的固定值。例如：

也可以是通过value传入的一个参数

这里base_id这个参数的值就是launch文件中arg标签声明的参数base_frame

robot_name不为空时执行的第二个group

第二个group内又加了一个group标签，这个标签对应的是ns。ns是ros下的命名空间机制。使用命名空间后，会在话题名前加上robot_name这个变量所对应的参数值。以odom话题为例如果传入的robot_name是robot1，查看话题列表显示的就会是/robot1/odom。这样是为例便于支持多机器人，防止话题名重名。

script文件夹：

接下来打开script文件夹下的base_control.py文件

首先是载入用到的python包，读取环境变量。

base_control功能包支持多款机器人，BASE_TYPE这个环境变量用来区分不同型号的机器人。从系统的环境变量读取BASE_TYPE这个值，根据不同型号的机器人做出不同的判断。读取SONAR_NUM的值判断是否支持超声波，以及超声波的数量。

接下来是一个queue的类，用来实现环形队列的读出和存储

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

get params部分：获取各种参数，这里获取的参数就是launch文件中param标签设置的参数。如果没有在launch文件中获取到这些参数就会使用类中提前设置的默认值。

例如pub_imu这个参数，在launch文件和节点中默认使用的都是False只有在启动时传入true到launch文件，节点再接收launch文件中param标签设置的参数。判断pub_imu为true才会在节点中设置imu的变量信息。

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

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

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

在launch文件中默认sub_ackermann为False

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

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

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

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

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

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

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

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

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

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

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

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

再往后就是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文件

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

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

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进行筛选。

6.2robot_vision功能包代码解析

打开Visual Studio Code进到工作空间下找到src下的robot_vision功能包，它包含config，data，launch，scripts四个文件夹。

config文件夹下存放的是配置相关的信息，data文件夹是我们自行创建的里面存放有人脸分类器和一张图片。

CMakeLists.txt文件：

在robot_vision功能包中有用到动态调参的功能，所以在CMakeLists.txt文件中

引用了动态调参的功能包dynamic_reconfigure

指定了参数配置文件

package.xml文件：

在robot_vision功能包下的package.xml在文件开头部位多了一个<package format="2">这是package标签的第二个版本，默认情况下使用的是format1，这两种的区别感兴趣的可以自行去ros官方网站了解。

这里多了一个 <build_depend>dynamic_reconfigure</build_depend>的编译依赖项，也是因为在robot_vision功能包中有用到动态调参的功能。

此外它没有run_depend标签而是使用exec_depend标签，这也是format2和format1的区别

launch文件夹：

launch文件夹下有一个opencv_apps文件夹，这里面存放的我们是根据opencv官方提供的demo手动编写的launch文件。

当我们进到opencv_apps功能包里面就会发现，这个功能包的scripts下只有一个人脸训练的python脚本。

这是因为apt方式安装的是二进制的可执行文件，要想获得源码需要在功能包的描述文件package.xml中获取。找到Wiki的链接http://wiki.ros.org/opencv_apps

在Wiki中就会有提供功能包的源码地址。

ar_track.launch：

这个launch在前面4.2.3节中有演示过实例。

这个launch文件中先启动了一个静态坐标转换的节点，然后就是launch文件中arg，param，node标签的基本使用，最后在末尾使用了remap标签。remap标签可以对话题做一个重命名。node标签指定的ar_track_alvar功能包用到的话题名是camera_image当你使用的话题名与其不符时就需要remap标签进行转换

fake_camera.launch：

fake_camera是我们在前面4.2.9节做图像叠加实例时用来虚拟发布一个相机话题。

这个launch文件启动的是我们自建的fake_camera.py节点，这个节点会根据传入的文件路径参数读取一个文件。也就是刚刚介绍的功能包data下的那张图片

line_follow.launch：

line_follow也是前面4.2.10节视觉循线讲到的实例，这个launch中启动了一个新的节点，它会将raw格式的图像话题转换成compressed的话题以节省带宽。

robot_camera.launch：

robot_camera这个launch文件的结构和功能实现与base_control.launch是类似的。先传入参数，然后使用了两个group，根据robot_name是否为空判断使用哪个group。robot_name非空则加上命名空间执行第二个group。在group中又根据机器人底盘类型做不同的处理，例如NanoRobot类型的摄像头是反装的，这就需要将horizontal_flip参数设为true

然后就是根据不同底盘类型做的一些静态坐标转换

script文件夹：

打开script文件夹下的cv_bridge_test.py文件

cv_bridge_test.py这个节点的作用是实现OpenCV中数据和ROS相互转换，演示实例在4.2.9节。节点首先订阅图像，然后使用imgmsg_to_cv2函数将订阅到的图像话题转换成OpenCV识别的图像格式。在OpenCV中处理完的图像再通过cv2_to_imgmsg函数转换成ros下的话题格式发布出去

face_detector.py：

script文件夹下有通过OpenCV来实现的人脸检测节点供开发参考感兴趣的可以自行运行

fake_camera.py：

fake_camera是实现一个虚拟摄像机然后发布一个话题用于图像叠加的节点。先通过OpenCV读取一张图片然后转换成ros的格式定期发布

line_detector.py：

循线节点的文件内容首先是订阅器，发布器以及CvBridge这个类的定义。

接下来是dynamic_reconfigure_callback这个动态调参的回调函数，每次通过rqt_reconfigure工具修改变量后就会调用这个回调函数。在这个回调函数中需要将rqt_reconfigure工具修改的参数赋予程序中的变量。

接下来在callback函数中会将图像的颜色，位置，中心点提取出来。再根据图像的中心的位置通过twist_calculate函数计算需要发布的速度指令的值

6.3雷达文件夹和robot_navigation功能包代码解析

lidar文件夹：

打开Visual Studio Code进到工作空间下找到src下的lidar文件夹。lidar是多个常用的雷达功能包的合集。其下包含hls_lfcd_lds_driver，iiiroboticslidar2_ros，rplidar_ros，sc_mini，ydlidar五款雷达的功能包。

此外lidar文件夹下还包含一个用于udev规则的生成的脚本initenv.sh它的作用是将设备进行udev规则转换让所有设备拥有统一的名称。

robot_navigation功能包：

功能包下除了几个熟悉的文件夹以外还多了rviz，maps，param三个文件夹。rviz中存放的是rviz相关的配置文件，maps存放的是地图文件和地图配置信息相关的文件，param存放的是yaml文件

我们先看launch文件夹，它下面有几个launch文件和includes，lidar，rviz三个文件夹。includes存放的是建图算法的launch文件，lidar存放的是雷达的launch文件，rviz存放的是rviz相关的launch文件

robot_slam_laser.launch：

robot_slam_laser.launch文件是运行激光SLAM建图时用到的launch文件。

它有四个可传入的参数，第一个可传入参数slam_methods在介绍4.3.4节激光SLAM建图算法切换时就有用到过，默认赋值gmapping可选值hector, karto, cartographer；第二个是open_rviz默认值false；第三个是simulation，它是判断启动实体机器人还是stage仿真器的标志默认值false；最后是planner，它的作用是在无地图环境下实现边建图边导航。默认参数为空，可选参数为dwa，teb两种局部路径规划器。

接下来是几个group当传入参数simulation为true时会启动仿真软件运行一个仿真机器人

当它为false时会启动launch文件夹下的robot_lidar这个launch

在robot_lidar这个launch文件中使用include标签包含了两个launch，分别是base_control和lidar。

lidar.launch的作用是，根据读取机器人底盘类型和雷达类型，做启动节点和TF坐标转换的判断。在lidar.launch中又根据雷达类型不同包含了不同类型的launch。这些launch都存放在功能包下launch文件夹下的lidar文件夹中。

回到robot_slam_laser.launch文件，在这里根据slam_methods参数选用的建图算法不同又包含了不同的launch文件。这些launch都存放在功能包下launch文件夹下的includes文件夹中。

当传入planner参数为dwa或teb，就会包含一个move_base.launch文件以实现无地图环境下导航建图功能

当open_rviz参数为true时会打开一个rviz界面，并使用功能包下rviz文件夹中预置的配置文件slam.rviz

以上就是robot_slam_laser.launch文件的大致内容，在这个文件中涉及到一些launch的循环调用。这样做的好处是可以相对的模块化，launch文件之间的相互嵌套避免了多次进行多个节点的启动。

robot_navigation.launch：

robot_navigation.launch和robot_slam_laser.launch是类似的，首先传入几个参数。

参数map_file传入的是maps文件下的map.yaml文件，这个文件的内容是地图名称，圆点，图像分辨率等基本配置信息。如果地图名称和保存路径是自定义的只需修改传入map_file的值即可。

simulation参数为true时，会通过仿真器在simulation_one_robot_with_map.launch文件中启动一个地图服务。 simulation参数为false时会启动robot_lidar也就是同时启动底盘和雷达，然后启动一个地图服务器并且读入map_file这个参数来获取地图文件。最后启动一个定位节点amcl

接下来这里包含了一个move_base.launch文件，这是一个导航堆栈。它有三个传入参数planner，simulation和use_dijkstra。

再看move_base.launch文件，默认传入的planner参数是dwa。由于robot_navigation.launch文件中有传入planner为teb，最终生效的参数是teb。teb和dwa是两个不同的局部路径规划算法，在这里使用了两个group进行区分。

当planner参数为dwa时，启动move_base功能包下的move_base节点然后载入一系列参数。我们先看costmap_common_params.yaml

以NanoCar车型为例，在功能包的param下的NanoCar文件夹找到这个文件，在这里需要设置机器人的矩形轮廓尺寸。footprint: [ [-0.035,-0.1], [0.18,-0.1], [0.18,0.1], [-0.035,0.1] ]表示以机器人中心为坐标轴原点，这四个坐标点分别对应机器人的四个角。路径规划器获取到机器人轮廓大小从而规划相应的路径

local_costmap_params.yaml：

在这个文件中需要指定发布频率和更新频率，为了降低机器人的计算负担设置了一个以机器人所在位置为中心的3*3滑动窗口，这个框设置得越小路径规划效果越差，越大机器人负担的运算就越大。最后设置了一个地图的层，第一个是静态层，也就是建图时的黑色边界以外的灰色可通行区域。第二个是障碍物层，地图中不存在但是实际扫面到的障碍物会被识别在障碍物层。

global_costmap_params.yaml：

这个文件也是一样设置一些发布频率等信息

move_base_params.yaml：

这个文件也是进行一些频率设置，包括对地盘的控制频率，路径规划频率以及机器人在一个点持续震荡时最大允许震荡时间等等。最后clearing_rotation_allowed这个参数的作用是，当机器人被困住时会原地360度转一圈重新扫描找到一条出路。这里我们的机器人以NanoCar是为例它不具备原地转向的能力所以需要禁用。

dwa_local_planner_params.yaml：

这个文件是导航时修改最多的参数，在这里可以设置机器人导航的最大最小速度，y轴的横向运动速度，转向角度，加速度信息，到达目标所能容忍的误差等等。

当planner参数为teb时，同样启动move_base功能包下的move_base节点加下来载入的参数也基本一致，区别在于最后载入的是teb_local_planner_params.yaml文件

teb_local_planner_params.yaml文件和dwa的基本类似，它将参数分开设置成转向速度，x轴速度，x轴后向的速度，y轴速度，y轴后向速度等等

teb路径规划支持阿克曼结构的机器人，所以需要设置最小转向半径的参数，轮距参数等。在使用仿真器仿真阿克曼车型时需要将cmd_angle_instead_rotvel参数值改为true。这是由于stage仿真器中”Car”车型接受的cmd_vel话题中z轴角速度并非给定机器人角速度，而是指转向结构的转向角度。

以上就是robot_navigation.launch的大致内容。

multi_points_navigation.launch：

这是多点导航的launch文件，它需要先启动导航的launch文件。在多点导航的launch中只启动了一个multi_goal_point.py的节点。

这个节点的作用是，通过rviz中的一个按钮先发布一个/goal目标点话题，而不是直接发布一个/move_base_simple/goal话题。/move_base_simple/goal话题会发送给move_base产生一个新的导航目标。通过MarkerArray将/goal这个目标点先缓存起来，move_base对每一个目标点导航的结果触发回调来执行下一个目标点实现多目标点导航

way_point.launch：

way_point.launch是机器人多点全自动巡航实例中用到的launch文件。它需要在参数中编写好预设的地点，然后启动launch文件。这个launch文件启动way_point.py这个节点

way_point.py这个节点会先读出launch文件传入节点的参数，然后将这些值处理成数组的形式，并判断几个数组的元素个数是否相等。条件满足时将各个数组，循环次数，地图基座标传入MultiGoals。MultiGoals这个类里面做的处理和多目标导航是十分类似的，它会根据传入的地点参数依次导航。

6.4robot_simulation功能包代码解析

robot_simulation是仿真器相关的功能包

simulation_one_robot.launch：

这个launch文件是启动机器人的仿真时用到的launch，它启动了stage_ros这个节点。这个节点运行了一个仿真器的环境，它需要一个仿真器的配置作为参数传入。我们以NanoCar为例看一下它的配置文件内容。

首先它包含了一个robot.inc文件，这个文件是用于描述我们所要仿真的机器人的信息。它定义了一个雷达传感器包括雷达扫描最远距离，扫描角度，雷达采样点数量以及雷达的尺寸。然后定义了机器人的位置，里程计的误差，机器人的尺寸，机器人的基座标与仿真器仿真出来的机器人立方体模型的偏移，机器人的颜色，机器人的运动学特性，机器人轴距，激光雷达到车身的变换等信息