T Thirteen
返回列表
ROS2+智能导航小车 2024年7月17日 · 34 分钟

项目二：激光雷达自动避障

基于 LD14 激光雷达扫描数据，实现小车实时检测障碍物并自动避障

一、 介绍

激光雷达工作原理

激光雷达的机械结构分为两个部分：一部分时固定底座，另一部分时可旋转的头部结构。在雷达的头部设置了一个红外激光发射器和一个红外激光接收器，当雷达工作的时候，会从发射器输出一道红外激光，这道光束击中障碍物后会反射回来，被雷达的接收器捕获。通过计时器测量激光发射和接收的间隔市场

时常×光速 = 飞行长度 飞行长度 / 2 = 障碍物距离

在这里插入图片描述 激光雷达测量玩一个方向的障碍物距离后，回旋装一个角度，再射出一道红外激光，并接收反射回来的光束，然后再旋转一个角度，一直重复这个操作，直至旋转一周

在这里插入图片描述

只要激光束探测的频率足够高，旋转速度足够快，就能实时的刷新周围障碍物的分布状况，于是我们再ROS的Rviz中，就能看到这样的画面，ROS中的激光雷达数据格式，就是对于这么一个障碍物轮廓点阵的具体描述 在这里插入图片描述

二、获取激光雷达数据

在机器人的ROS系统中，激光雷达通常会有一个对应的节点，这个节点一般由雷达的厂商提供， 雷达的测距数值从电路系统传递到雷达节点，然后回封装成一个消息包，发布咋爱一个Topic话题中， 我们只需要订阅这个话题就能获取激光雷达的数据。

在这里插入图片描述

2.1 实现步骤：

  • 构建一个新的功能包，包名叫做lidar_behavior
  • 在软件包中新建一个节点，节点名为lidar_data.py
  • 在节点中，申请订阅话题/sacn，并设置回调函数为lidar_callback()
  • 构建回调函数lidar_callback()，用来接收和处理雷达数据
  • 显示雷达检测到的前方障碍物距离

2.2 编写代码

构建一个新的功能包lidar_behavior

ros2 pkg create --build-type ament_python lidar_behavior

在这里插入图片描述

在lidar_behavior文件夹中新建一个文件

lidar_data.py

#!/usr/bin/env python3
# coding=utf-8

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan

# 创建一个节点类
class LidarDataNode(Node):
    def __init__(self):
        super().__init__('lidar_data')
        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.lidar_callback,
            10)
        self.subscription  # 防止未使用的变量警告

    # 激光雷达回调函数
    def lidar_callback(self, msg):
        number = len(msg.ranges)
        self.get_logger().info("雷达测距数量 = %d" % number)
        middle = number // 2
        self.get_logger().warn("正前方测距数值 = %f 米" % msg.ranges[middle])

def main(args=None):
    rclpy.init(args=args)
    node = LidarDataNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

修改 package.xml

添加下面的内容

  <exec_depend>rclpy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>

完整如下：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>lidar_behavior</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="lll@todo.todo">lll</maintainer>
  <license>TODO: License declaration</license>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>


  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

修改setup.py

from setuptools import setup
import os
from glob import glob

package_name = 'lidar_behavior'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'scripts'), glob('scripts/*.py')),
        (os.path.join('share', package_name, 'lidar_behavior'), glob('lidar_behavior/*.py')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='lll',
    maintainer_email='lll@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'lidar_data = lidar_behavior.lidar_data:main',
        ],
    },
)

2.3 编译运行

colcon build
source install/setup.bash
ros2 run lidar_behavior lidar_data

在这里插入图片描述

三、实现激光雷达避障

我们只需要将运动控制的部分加进来就可以了 在这里插入图片描述

3.1 实现步骤

  • 发布速度控制话题/cmd_vel
  • 构建速度控制消息包vel_cmd
  • 根据激光雷达的测距数值，实时调整机器人运行速度，避开障碍物

3.2 编写代码

新建lidar_behavior.py

#!/usr/bin/env python3
# coding=utf-8

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class LidarBehavior(Node):
    def __init__(self):
        super().__init__('lidar_behavior')
        self.vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.lidar_sub = self.create_subscription(LaserScan, 'scan', self.lidar_callback, 10)
        self.count = 0

    def lidar_callback(self, msg):
        middle = len(msg.ranges) // 2
        dist = msg.ranges[middle]
        self.get_logger().info("正前方测距数值 = %f 米" % dist)
        vel_cmd = Twist()
        if self.count > 0:
            self.count -= 1
            self.get_logger().warn("持续转向 count = %d" % self.count)
            return
        if dist < 1.5:
            vel_cmd.angular.z = 0.3
            self.count = 50
        else:
            vel_cmd.linear.x = 0.05
        self.vel_pub.publish(vel_cmd)

def main(args=None):
    rclpy.init(args=args)
    node = LidarBehavior()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

修改setup.py

    entry_points={
        'console_scripts': [
            'lidar_behavior = lidar_behavior.lidar_behavior:main',
            'lidar_data = lidar_behavior.lidar_data:main',
        ],
    },

3.3 编译运行

colcon build
source install/setup.bash
ros2 run lidar_behavior lidar_behavior

在这里插入图片描述

四、启动小车

ssh远程登录到树莓派后的终端

ssh user@192.168.1.100
# 然后输入密码即可登录

随后使用tmux分割终端，一共需要3个终端

# 切换窗口
ctrl+b c: 创建一个新窗口（状态栏会显示多个窗口的信息）
ctrl+b p: 切换到上一个窗口（按照状态栏的顺序）
ctrl+b n: 切换到下一个窗口
ctrl+b w: 从列表中选择窗口（这个最好用）

4.1 第一个终端：

小车控制

cd ros2_car_ws
source install/setup.bash
ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6

4.2 第二个终端：

雷达话题发布

cd ros2_car_ws
source install/local_setup.bash
ros2 launch ldlidar_sl_ros2 ld14.launch.py

4.3 第三个终端：

激光雷达避障节点

cd ros2_car_ws
source install/local_setup.bash
ros2 run lidar_behavior lidar_behavior

五、可能会出现的问题

编译ldlidar_sl_ros2 节点时可以能出现的问题导致编译失败

lll@laj:~/ros2_car_ws$ colcon build --packages-select ldlidar_sl_ros2-master
[0.278s] WARNING:colcon.colcon_core.package_selection:ignoring unknown package 'ldlidar_sl_ros2-master' in --packages-select
                     
Summary: 0 packages finished [0.25s]
lll@laj:~/ros2_car_ws$ colcon build --packages-select lidar_behavior
Starting &gt;&gt;&gt; lidar_behavior
--- stderr: lidar_behavior                   
Sorry: IndentationError: unindent does not match any outer indentation level (lidar_follower.py, line 25)
---
Finished &lt;&lt;&lt; lidar_behavior [2.25s]

Summary: 1 package finished [2.50s]
  1 package had stderr output: lidar_behavior
lll@laj:~/my_robot$ colcon build 
Starting &gt;&gt;&gt; ldlidar_sl_ros2
--- stderr: ldlidar_sl_ros2                               
/home/lll/my_robot/src/ldlidar_sl_ros2/ldlidar_driver/src/log_module.cpp: In member function ‘void LogModule::InitLock()’:
/home/lll/my_robot/src/ldlidar_sl_ros2/ldlidar_driver/src/log_module.cpp:172:3: error: ‘pthread_mutex_init’ was not declared in this scope; did you mean ‘pthread_mutex_t’?
  172 |   pthread_mutex_init(&amp;mutex_lock_,NULL);
      |   ^~~~~~~~~~~~~~~~~~
      |   pthread_mutex_t
/home/lll/my_robot/src/ldlidar_sl_ros2/ldlidar_driver/src/log_module.cpp: In member function ‘void LogModule::RealseLock()’:
/home/lll/my_robot/src/ldlidar_sl_ros2/ldlidar_driver/src/log_module.cpp:180:9: error: ‘pthread_mutex_unlock’ was not declared in this scope; did you mean ‘pthread_mutex_t’?
  180 |         pthread_mutex_unlock(&amp;mutex_lock_);
      |         ^~~~~~~~~~~~~~~~~~~~
      |         pthread_mutex_t
/home/lll/my_robot/src/ldlidar_sl_ros2/ldlidar_driver/src/log_module.cpp: In member function ‘void LogModule::Lock()’:

此时打开/ldlidar_sl_ros2/ldlidar_driver/src/log_module.cpp文件

在开头添加即可

#include <pthread.h>