课程4.标定结果测试及激光传感器安装与显示 - TonyRobotics/trbase GitHub Wiki

本节将对上节标定的运动参数进行多方位测试，将所有参数放到launch文件中进行修改调试。待参数满足要求精度后，即可作为底盘的驱动程序。 然后，我们将列举几款常用的激光传感器，并对其中一款进行安装和显示。

将所有参数放到launch文件中

修改trd_driver.py文件，将所有参数用ROS的get_param()方法获取：

self.wheel_diameter = rospy.get_param('~wheel_diameter', default=0.1)
self.base_width = rospy.get_param('~base_width', default=0.5)
self.encoder_ticks_per_rev = rospy.get_param('~encoder_ticks_per_rev', default=5000)
self.linear_coef = rospy.get_param('~linear_coef', default=800.0)
self.angular_coef = rospy.get_param('~angular_coef', default=200.0)

v1 = self.linear_coef*msg.linear.x
v2 = self.linear_coef*msg.linear.x
v1 += self.angular_coef*msg.angular.z
v2 -= self.angular_coef*msg.angular.z

修改后内容如下：

#!/usr/bin/python2
# coding: utf-8

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf

import serial
import time
import math

from threading import Lock
lock = Lock()

class TrdDriver():

    def __init__(self, serialport, baudrate):
        self.pub = rospy.Publisher('/odom', Odometry, 
                                   queue_size=100) 
        self.sub = rospy.Subscriber('/cmd_vel', Twist, 
                                    self.vel_callback, 
                                    queue_size=1)
        self.ser = serial.Serial(serialport, baudrate)

        self.wheel_diameter = rospy.get_param('~wheel_diameter', default=0.1)
        self.base_width = rospy.get_param('~base_width', default=0.5)
        self.encoder_ticks_per_rev = rospy.get_param('~encoder_ticks_per_rev', default=5000)
        self.linear_coef = rospy.get_param('~linear_coef', default=800.0)
        self.angular_coef = rospy.get_param('~angular_coef', default=200.0)

        self.first_flag = True
        self.encoder1_offset = 0
        self.encoder2_offset = 0
        self.encoder1 = 0
        self.encoder2 = 0
        self.encoder1_prev = 0
        self.encoder2_prev = 0

        self.x = 0
        self.y = 0
        self.theta = 0

        self.odom = Odometry()
        self.odom.header.frame_id = 'odom'
        self.odom.child_frame_id = 'base_link'

        self.time_prev = rospy.Time.now()

    def send(self, cmd):
        self.ser.write(cmd)

    def read_buffer(self):
        time.sleep(0.05)
        res = ''
        while self.ser.inWaiting() > 0:
            res += self.ser.read(1)
        res = bytearray(res)
        if res[0:2]=='?C' and res[-1]==13:
            self.encoder1 = int(res.split(':')[0].split('=')[1])
            self.encoder2 = -int(res.split(':')[1][:-1])
            if self.first_flag:
                self.encoder1_offset = self.encoder1
                self.encoder2_offset = self.encoder2
                self.first_flag = False
            self.encoder1 -= self.encoder1_offset
            self.encoder2 -= self.encoder2_offset
            print('encoder', self.encoder1, self.encoder2)
        return res

    def get_encoder(self):
        cmd = '?C\r'
        self.send(cmd)

    def set_speed(self, v1, v2):
        print('{} Set Speed: {} {}'.format(time.time(), v1, v2))
        cmd = '!M {} {}\r'.format(v1, v2)
        self.send(cmd)

    def update_odom(self):
        encoder1 = self.encoder1
        encoder2 = self.encoder2
        time_current = rospy.Time.now()
        time_elapsed = (time_current - self.time_prev).to_sec()
        self.time_prev = time_current
        dleft = math.pi * self.wheel_diameter * \
                (encoder1 - self.encoder1_prev) / self.encoder_ticks_per_rev
        dright = math.pi * self.wheel_diameter * \
                (encoder2 - self.encoder2_prev) / self.encoder_ticks_per_rev
        self.encoder1_prev = encoder1
        self.encoder2_prev = encoder2
        d = (dleft + dright) / 2
        dtheta = (dright - dleft) / self.base_width
        if d != 0:
            dx = math.cos(dtheta) * d
            dy = -math.sin(dtheta) * d
            self.x += dx*math.cos(self.theta)-dy*math.sin(self.theta)
            self.y += dx*math.sin(self.theta)+dy*math.cos(self.theta)
        self.theta += dtheta

        self.odom.header.stamp = time_current
        self.odom.pose.pose.position.x = self.x
        self.odom.pose.pose.position.y = self.y
        q = tf.transformations.quaternion_from_euler(0,0,self.theta)
        self.odom.pose.pose.orientation.x = q[0]
        self.odom.pose.pose.orientation.y = q[1]
        self.odom.pose.pose.orientation.z = q[2]
        self.odom.pose.pose.orientation.w = q[3]
        self.odom.twist.twist.linear.x = d / time_elapsed
        self.odom.twist.twist.angular.z = dtheta / time_elapsed

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            # 读编码器值
            lock.acquire()
            self.get_encoder()
            self.read_buffer()
            lock.release()
            # 更新计算并更新里程信息
            self.update_odom()
            # 发布里程信息
            self.pub.publish(self.odom)
            rate.sleep()

    def vel_callback(self,msg):
        lock.acquire()
        v1 = self.linear_coef*msg.linear.x
        v2 = self.linear_coef*msg.linear.x
        v1 += self.angular_coef*msg.angular.z
        v2 -= self.angular_coef*msg.angular.z
        self.set_speed(v1, -v2)
        self.read_buffer()
        lock.release()

if __name__=='__main__':
    rospy.init_node('trd_driver001')
    serialport = rospy.get_param('~serialport', default='/dev/ttyUSB0')
    baudrate = rospy.get_param('~baudrate', default=115200)
    trd_driver = TrdDriver(serialport, baudrate)
    trd_driver.run()

然后，修改trd_control.launch文件，加入对应参数数值。 注意，此处底盘的轮径为0.07米，轮距为0.4米，编码器为4000线，控制速度线速度系数和角速度系数分别为800和200。

<?xml version="1.0"?>
<launch>
    <arg name="serialport" default="/dev/ttyUSB0" />
    <arg name="baudrate" default="115200" />
    <arg name="wheel_diameter" default="0.07" />
    <arg name="base_width" default="0.4" />
    <arg name="encoder_ticks_per_rev" default="4000" />
    <arg name="linear_coef" default="800" />
    <arg name="angular_coef" default="200" />

    <node name="trd_driver" pkg="trd_driver" type="trd_driver.py">
        <param name="serialport" value="$(arg serialport)" />
        <param name="baudrate" value="$(arg baudrate)" />
        <param name="wheel_diameter" value="$(arg wheel_diameter)" />
        <param name="base_width" value="$(arg base_width)" />
        <param name="encoder_ticks_per_rev" value="$(arg encoder_ticks_per_rev)" />
        <param name="linear_coef" value="$(arg linear_coef)" />
        <param name="angular_coef" value="$(arg angular_coef)" />
    </node>
</launch>

测试参数是否满足精度

启动驱动节点和键盘控制节点，遥控底盘运动，并查看/odom主题内容。

(远程终端)$ rosrun trd_driver trd_driver.py

(远程终端)$ rosrun trd_driver keyboard_teleop.py

(远程终端)$ rostopic echo /odom

观察/odom中的Position位置读数以及速度Twist读数，看看发现什么问题？如何调整参数进行校准？

思考：如何测试，可以确保上述参数是准确的？

最终参数：

    <arg name="wheel_diameter" default="0.08" />
    <arg name="base_width" default="0.4" />
    <arg name="encoder_ticks_per_rev" default="4000" />
    <arg name="linear_coef" default="800" />
    <arg name="angular_coef" default="180" />

激光传感器的介绍

激光传感器的作用是，扫描机器人周围的物体，并以点云的形式进行表示。 因此，激光传感器分为二维激光传感器和三维激光传感器，会分别提供二维点云和三维点云数据。

通常，三维激光传感器价格昂贵，移动底盘通常会采用二维激光传感器。常用的型号有：

  • 日本北洋Hokuyo系列

ROS驱动：http://wiki.ros.org/urg_node

  • 德国SICK系列

ROS驱动：http://wiki.ros.org/sick_tim

  • 深圳镭神智能生产的激光雷达

ROS驱动：https://github.com/leishen-lidar/LS01D

  • 上海思岚科技生产的RPLIDAR系列

ROS驱动：http://wiki.ros.org/rplidar

  • 深圳EAI公司生产的YDLIDAR系列

ROS驱动：https://github.com/EAIBOT/ydlidar

示例一：安装YDLIDAR F4激光ROS驱动包

在此，采用激光传感器型号为深圳EAI公司推出的YDLIDAR F4型号。其ROS驱动包可在Github下载：

https://github.com/EAIBOT/ydlidar

将源代码下载到笔记本电脑catkin_ws工作区的src路径下，然后部署到Odroid XU4上：

$ cd ~/catkin_ws/src
$ git clone https://github.com/EAIBOT/ydlidar

然后执行rsync指令同步代码。在此，可将rsync指令写到一个.sh脚本中，每次执行脚本文件就可以了。 在catkin_ws下新建deploy.sh文件，加入以下内容：

rsync -avz --delete --exclude="*.swp" src [email protected]:/home/odroid/catkin_ws/

远程部署时在catkin_ws下执行即可。

$ cd ~/catkin_ws
$ sh deploy.sh

将代码部署到Odroid XU4后，登陆上去进行构建、配置。

$ ssh [email protected]
(远程终端)$ cd ~/catkin_ws
(远程终端)$ catkin_make

构建完成后，即可运行测试。为了防止串口名称的冲突，可执行以下命令，将激光传感器映射为特定的名称。

(远程终端)$ roscd ydlidar/startup
(远程终端)$ sudo sh initenv.sh

上述指令会建立udev rules文件，将激光串口名映射为/dev/ydlidar，方便后续调用。

注意：设置完udev rules后，需要重新连接激光传感器才会生效。

示例二：安装镭神ls01d激光ROS驱动包

同理，如果采用镭神生产的ls01d激光传感器，则需要下载安装其对应驱动。

首先到github下载驱动包，地址为https://github.com/leishen-lidar/LS01D

$ git clone https://github.com/leishen-lidar/LS01D

然后将其中对应的ROS驱动包重命名，即将ls01D Ros文件夹重命名为ls01d， 将重命名后的ls01d文件夹拷贝到工作区的~/catkin_ws/src文件夹下。

同样，将代码远程部署到Odroid XU4后，登陆上去进行构建、配置。

$ ssh [email protected]
(远程终端)$ cd ~/catkin_ws
(远程终端)$ catkin_make

构建完成后，即可运行测试。为了防止串口名称的冲突，可执行以下命令，将激光传感器映射为特定的名称。

(远程终端)$ roscd ls01d/scripts
(远程终端)$ sudo sh create_udev_rules.sh

上述指令会建立udev rules文件，将激光串口名映射为/dev/laser，方便后续调用。

注意：设置完udev rules后，需要重新连接激光传感器才会生效。

示例三：安装Hokuyo激光驱动包

由于Hokuyo激光传感器的ROS驱动包已发布到ROS的Ubuntu软件源中，所以可以直接二进制方式安装：

$ sudo apt install ros-kinetic-urg-node

同理，SICK的激光驱动包也可通过二进制方式安装：

$ sudo apt install ros-kinetic-sick-tim

启动激光驱动，查看发布的消息

将激光传感器连接到Odroid XU4的USB端口上，用roslaunch启动驱动节点：

(远程终端)$ roslaunch ydlidar lidar.launch

或:

(远程终端)$ roslaunch ls01d ls01d.launch

在新打开终端，查看发布的主题及内容：

(远程终端)$ rostopic list
(远程终端)$ rostopic echo /scan

这样可看到打印出激光点云数据。请自行查看消息的类型，及其定义，各变量所表示的含义。

ROS多机运行的配置

为了能够在笔记本电脑上也能接收到激光数据，需要配置两台机器的ROS环境变量参数。

以底盘Odroid XU4作为ROS主节点。 在Odroid XU4的~/.bashrc文件加入以下内容。

export ROS_MASTER_URI=http://192.168.5.102:11311
export ROS_HOSTNAME=192.168.5.102

然后，在笔记本电脑的~/.bashrc文件加入以下内容。

export ROS_MASTER_URI=http://192.168.5.102:11311
export ROS_HOSTNAME=192.168.5.101

这样，在Odroid XU4上启动roscore后及相关节点后，即可在笔记本电脑上进行查看和通信，请自行尝试。

  • 尝试在Odroid XU4启动激光驱动，并在笔记本电脑打印其内容。

  • 尝试在笔记本电脑发送某主题，并在Odroid XU4上打印其内容。

使用RViz查看激光信号

用打印的方式查看激光信号很不直观，这时，用RViz可直观的查看激光信号。

在Odroid XU4启动激光驱动后，在笔记本电脑运行RViz：

(远程终端)$ roslaunch ydlidar lidar.launch

或：

(远程终端)$ roslaunch ls01d ls01d.launch

$ rviz

在RViz界面中，选择Add按钮，在By Topic选项中找到/scan主题，选中并添加。

这时，RViz不会显示激光数据。需要将Fixed Frame改为laser_frame即可显示。为了显示更清晰，将LaserScan下的Style改为Points。

自行修改其它选项，看RViz都提供了哪些功能？

补充：如何解决usb断线重连问题

由于坦克底盘运行过程中会出现颠簸，难免导致USB接口出现断开的情况。因此需要在代码中加入重连机制。

提示 在TrdDriver类中定重连成员方法：

def reconnect(self):
    print('{} reconnecting...'.format(time.time()))
    while True:
        try:
            self.ser = serial.Serial(self.serialport, self.baudrate)
        except:
            time.sleep(1)
            print('{} reconnecting...'.format(time.time()))
        else:
            print('{} reconnected!'.format(time.time()))
            break

提示 发送指令出现异常时重连：

def send(self, cmd):
    while True:
        try:
            self.ser.write(cmd)
            break
        except serial.serialutil.SerialException:
            self.reconnect()

提示 接收指令出现异常时重连：

res = ''
try:
    while self.ser.inWaiting() > 0:
        res += self.ser.read(1)
except:
    self.reconnect()

让创造机器人更便捷！

⚠️ **GitHub.com Fallback** ⚠️