1 引言
前面的两篇推文:《开源!手把手教你搭建Arduino+英伟达Jetson的ROS小车(上)》中,我们介绍了一台Jetson-nano小车所需要的硬件部分;《开源!手把手教你驱动Arduino+ROS小车的电机》中,我们介绍了如何使用Arduino及配套的扩展板实现两个直流减速电机的差速控制与PID调速。
那么今天这一篇,我们将讲解如何在Jetson Nano中配置ros_arduino_bridge、如何配置和使用 imu 、如何使用 CSI 摄像头。
亚克力-TT马达版本底盘
全套亚克力-37电机版本
本次主要以我们设计的黑色亚克力版本为例,教大家如何配置软件部分。具体步骤请让我娓娓道来。
2 真机平台
-
Jetson Nano + Arduino mega 2560 驱动的两轮差速小车
ps: 带有四个转向灯,程序已合并更新
-
树莓派摄像头 (V2 版本)
-
可旋转摄像头支架(3D 打印件)
-
测试赛道
-
9轴 IMU
3 调试 ros_arduino_bridge 程序
这个程序是运行在 ubuntu + ros melodic系统下的,也就是说这里的Jetson Nano 所运行的系统里面需要安装相应的运行环境。这里就不再描述如何给 Jetson Nano 烧录系统与安装ROS了,Nvidia对应的网址有详细的教程。这里主要是记录一下自己是如何配置 ros_arduino_bridge 这个ROS 包的。
-
ros_arduino_bridge 文件结构
ps: win10 cmd 命令为 tree /F
上图左侧解读如下:
ros_arduino_bridge: 存放的是该功能包的 编译文件.txt 和 依赖记录文件.xml
ros_arduino_firmware: 这个是大佬开源的时候 给定的一个 arduino 模板框架
ros_arduino_msgs: 存放的是自定义的 消息 msg 和 服务 srv 文件
ros_arduino_python: 存放的是 和 arduino 完成串口数据交互的 python ROS 节点程序
上面红色数字依次解读如下:
-
1 my_arduino_params.yaml
根据上图中的介绍,自行更改配置文件中的每一个参数,以适配自己的车辆,其中(timeout. rate. sensorstate_rate. base_controller_rate) 可保持不变。
-
2 arduino_driver.py
该 .py 文件里面,原作者写好了一些对串口进行的操作函数:
打开\关闭串口:
def open(self):
''' Open the serial port.
'''
self.port.open()
def close(self):
''' Close the serial port.
'''
self.port.close()
串口接收\发送函数:
def send(self, cmd):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
self.port.write(cmd + '\r')
def recv(self, timeout=0.5):
timeout = min(timeout, self.timeout)
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner. Note: we use read() instead of readline() since
readline() tends to return garbage characters from the Arduino
'''
c = ''
value = ''
attempts = 0
# 判断发送是否完毕
while c != '\r':
c = self.port.read(1)
value += c
attempts += 1
if attempts * self.interCharTimeout > timeout:
return None
value = value.strip('\r')
return value
该文件中还有很多基于串口操作的函数,为了简化前期学习熟悉成本,我们对 Arduino 的代码做了细微修改,只留下了实现ROS小车所需的主要功能。但同时又为了大家后期方便深入学习与定制功能,我们保留了 arduino_driver.py 中的函数操作。
-
3 arduino_sensors.py
后续增补
-
4 base_controller.py
该文件主要是订阅其他节点发布的速度话题数据 “cmd_vel”,然后通过实体小车的各个参数,将速度分解为小车电机调速实际所需的脉冲数目;与此同时,也根据电机编码器实际反馈的编码值推算小车的航迹信息数据“wheel_odom”。分别对其说明如下:
速度( cmd_vel ) --> 电机所需的目标编码值:
# cmd_vel 话题的回调函数
# self.encoder_resolution 编码器分辨率
# self.gear_reduction 电机减速度比
# self.wheel_diameter 轮胎直径
# ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)( 脉冲数 / 米 )
def cmdVelCallback(self, req):
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# 还有旋转 / 原地旋转
right = th * self.wheel_track / 2.0
left = -right
elif th == 0:
# 只有前进倒退
left = right = x
else:
# 以一定的线速度和角速度 运动
left = x - th * self.wheel_track / 2.0 # self.wheel_track 轮间距
right = x + th * self.wheel_track / 2.0
# self.arduino.PID_RATE PID执行的频率
self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
电机实际的编码值 --> 航迹信息 ( wheel_odom ):
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec() #时间单位转换成 秒s .
# 根据相邻采样时间内收到的脉冲数 推算每一个轮胎各自前进了多少 米 m .
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
# 通过差速小车的各个轮子行程差异 求解 合位移 .然后再求合速度
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
# 根据合位移 合速度求解 小车在世界坐标系下的行程 x y 方向。
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave #+
dy = sin(dth) * dxy_ave #-
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
# 求解 odom 话题数据 的姿态数据
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# 赋值 && 准备发布里程计话题数据 。
odom = Odometry()
odom.header.frame_id = "wheel_odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
# 设定协方差数据,目的在融合 imu 的时候好区分。
if vxy == 0 and vth == 0 :
odom.pose.covariance = ODOM_POSE_COVARIANCE2
odom.twist.covariance = ODOM_TWIST_COVARIANCE2
else :
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
以上的代码,已经全部上传至我们的开源仓库啦,欢迎下载体验啦:GitHub - COONEO/Arduino_Jetson_nano_ROS_Car: Making a Jetson_nano robot by yourself step by step.
https://github.com/COONEO/Arduino_Jetson_nano_ROS_Car.git
4 调试 IMU 程序
-
使用 IMU 的目的
这里主要是因为单纯使用轮式里程计进行gmapping建图时,在转弯的时候会出现地图漂移的现象,比较常见的办法就是使用一个imu数据来融合轮式里程计,将融合后的里程计数据输入到gmapping建图方法,实现建图功能。
我们挑选的IMU模块儿是9轴带温度补偿的,相比6轴它具备磁力计,该传感器可以有效矫正Z轴旋转方向的零飘,使得融合后的里程计数据更加好。通过查看ROS中关于imu数据的定义如下:
Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z
-
配置 IMU
结合上述的定义和目前实际的使用需求,我们通过阅读IMU的通讯协议,写了一个将IMU模块输出的数据转换成ROS imu 话题数据的ROS包。在运行该包之前,需要使用厂家提供的Win10 平台上位机对IMU模块进行设置。具体的设置步骤如下:
参照“imu_901/resource_folder”文件夹中的使用说明书,并借助win10上位机操作软件:“MiniIMU.exe” 对模块进行如下配置:
1、修改波特率为:115200
2、修改配置中输出项目为:加速度、角速度、角度、磁场、四元数。
3、修改配置中的输出频率为:200Hz
4、按照官网中的视频教程,利用“MiniIMU.exe”软件对模块进行校准(加速度、角度、磁场)。教程视频如下:
http://wiki.wit-motion.com/doku.php?id=jy901%E8%B5%84%E6%96%99
http://wiki.wit-motion.com/doku.php?id=jy901%E8%B5%84%E6%96%99
-
ROS下使用该IMU
该程序均已放置在我们的仓库中啦,自行下载网址如下:
GitHub - COONEO/Arduino_Jetson_nano_ROS_Car: Making a Jetson_nano robot by yourself step by step.
https://github.com/COONEO/Arduino_Jetson_nano_ROS_Car.git
将其中的 catkin_ws 文件夹放置到你的Home目录下
cd catkin_ws
rosdep install --from-paths src --ignore-src -r -y
catkin_make -j
source devel/setup.bash
使用USB-Type C 数据线,将电脑和imu连接起来,查看 /dev 目录下,分配的虚拟端口号(一般为:ttyUSB*),如果只有插入了一个设备,那么一定是: ttyUSB0。如我电脑此时截图的右下角所示:
如果您使用的是我们的Jetson Nano 环境,那么已经通过IMU的ID进行了重映射了,且新的名字为 IMU_PORT ,并且imu_901.launch文件中也是打开的该端口。若您的主机没有,则建议查看我们的推文《搭建ROS机器人之——ubuntu绑定串口设备》,即可绑定串口并重映射。
上图的 imu_901.launch 文件运行了imu_complementary_filiter_node 、tf 、imu_901_node 等节点。其中两个tf描述的是imu话题数据和坐标系 base_link 之间的位置关系(三轴平移+三轴旋转)。温馨提示:这里指定了base_link坐标系的原点在两轮差速小车的驱动轮中间位置,且X朝前、Y朝左、Z朝上。另外,因为小车一般携带屏幕,所以就默认把Rviz可视化节点给注释掉了。运行过程如下:
另外,使用:rostopic list 可以得知 imu_901 发布的话题数据:
其中:
imu 是姿态信息:
mag 是磁力计信息;
两者都是以200Hz的频率发布的。
-
以上的代码,已经全部上传至我们的开源仓库啦,欢迎下载体验啦:GitHub - COONEO/Arduino_Jetson_nano_ROS_Car: Making a Jetson_nano robot by yourself step by step.
https://github.com/COONEO/Arduino_Jetson_nano_ROS_Car.git
5 调试 树莓派CSI摄像头 V2版本
由于Jetson Nano 中无法直接通过usb_cam ROS包获取CSI接头的摄像头,经过网上查询,发现了一个大牛开源了一个程序 gscam ROS包,网址如下:
http://wiki.ros.org/gscam
我们已经适配好了摄像头的ROS启动文件,只需要插上CSI接头的树莓派V2摄像头,即可通过运行对应的launch文件,获得该摄像头的摄像头视频流。具体过程如下:
cd catkin_ws
# 若未编译,则 catkin_make
source devel/setup.bash
roslaunch jetson_nano_csi_cam_ros jetson_csi_cam.launch
ps:这里是ssh进的Jetson Nano主机,没有屏幕的小伙伴可以尝试这样
此时在本地终端 通过rostopic list 即可看见如下输出:
另外,你还可以通过 rqt_image_view 查看图像。
rosrun rqt_image_view rqt_image_view
6 展望:
在后面的推文中,我们将继续更新:建图篇章、导航篇章、摄像头应用篇章,其实Jetson Nano 版本的和 Raspberry Pi版本的程序差异不大,如果是想更快实现上述功能,也可以参照我们的树莓派ROS小车的推文和代码仓库。
https://github.com/COONEO/Arduino_Raspberry_ROS_Car.git # 已经暂时完结
https://github.com/COONEO/Arduino_Jetson_nano_ROS_Car.git # 正在陆陆续续更新中
创作不易,如果喜欢这篇内容,请您也转发给您的朋友,一起分享和交流创造的乐趣,也激励我们为大家创作更多的机器人研发攻略,让我们一起learning by doing!