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

项目三：激光雷达自动跟随

选定前方最近目标并保持固定距离，实现基于激光雷达的自动跟随控制

一、介绍

激光雷达跟随其实简单来说就是选择前方范围内一个最近的物体，并始终和这个物体保持一定的距离，而我们怎么选择这个范围呢？

LD14P 使用左手坐标系，旋转中心为坐标原点，传感器的正前方定义为零度方向，旋转角度沿着顺时针方向增大，具体如下图所示：

从下图中可以看到雷达的角度信息，雷达顺时针旋转发射光束，我们就可以选择一个角度范围来获取这个范围内的数据，比如正前方（-30，30），正后方（150，210），我们就可以使用这种方法来实现自动跟随的功能。

在这里插入图片描述 ROS2 激光雷达跟随的原理可以总结为以下几个步骤

  • 初始化：初始化节点和参数，创建激光雷达数据订阅器和速度指令发布器，初始化 PID 控制器。
  • 激光雷达数据处理：订阅激光雷达数据，提取并处理前方特定角度范围内的距离数据。
  • PID 控制计算：利用 PID 控制器计算所需的速度调整。
  • 发布速度指令：发布速度指令以控制机器人移动，确保机器人与目标保持设定的距离。

二、实现激光雷达自动跟随

2.1 实现步骤：

  • 构建一个新的功能包，包名叫做lidar_follower
  • 在软件包中新建一个节点，节点名为lidar_follower.py
  • 在节点中，申请订阅话题/sacn，并设置回调函数为lidar_callback()
  • 构建回调函数lidar_callback()，用来接收和处理雷达数据
  • 显示雷达检测到的前方障碍物距离
  • 使用 PID 控制器根据目标距离和当前距离（误差）计算需要的速度调整。
  • 发布速度指令，控制机器人移动

2.2 编写代码

构建一个新的功能包lidar_follower

ros2 pkg create --build-type ament_python lidar_follower

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

lidar_follower.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
import numpy as np

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

    def lidar_callback(self, msg):
        # 获取前方 150 到 210 度范围内的激光数据
        ranges = np.array(msg.ranges)
        ranges = np.where(np.isnan(ranges), np.inf, ranges)  # 将 NaN 值替换为无穷大
        angle_min = msg.angle_min
        angle_increment = msg.angle_increment
        angles = np.arange(angle_min, angle_min + len(ranges) * angle_increment, angle_increment)
        
        # 将角度范围转换为弧度
        angle_min_rad = np.deg2rad(150)
        angle_max_rad = np.deg2rad(210)
        
        # 只取 150 到 210 度范围内的数据
        front_indices = np.where((angles >= angle_min_rad) & (angles <= angle_max_rad))
        front_distances = ranges[front_indices]
        front_angles = angles[front_indices]
        
        # 计算距离最近的目标位置
        if len(front_distances) > 0 and not np.all(np.isinf(front_distances)):
            min_distance_index = np.argmin(front_distances)
            min_distance = front_distances[min_distance_index]
            target_angle = front_angles[min_distance_index]
        else:
            min_distance = float('inf')
            target_angle = 0.0

        self.get_logger().info("目标距离 = %f 米, 目标角度 = %f 弧度" % (min_distance, target_angle))

        # 根据目标距离和角度生成速度指令
        vel_cmd = Twist()
        target_distance = 0.5  # 目标距离
        distance_tolerance = 0.05  # 允许的误差范围
        
        if min_distance < target_distance - distance_tolerance:
            vel_cmd.linear.x = -0.2  # 后退
        elif min_distance > target_distance + distance_tolerance:
            vel_cmd.linear.x = 0.2  # 前进
        else:
            vel_cmd.linear.x = 0.0  # 停止

        # 设置角速度为目标角度的一部分，以便对准目标
        # vel_cmd.angular.z = target_angle * 0.5

        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()

修改 package.xml

添加下面的内容

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

修改setup.py

from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'lidar_follower'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    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_follower/*.py')),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.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_follower = lidar_follower.lidar_follower:main',
            'lidar_followers = lidar_follower.lidar_followers:main',
        ],
    },
)

2.3 编译运行

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

到这里以及实现了雷达跟随的功能，但我们之前每次使用时都需要先运行雷达数据发布节点，再用另一个终端接收这个话题数据并实现功能，有没有一种办法可以将这两部放到一个终端中，用一个命令来调用两个功能。

因为接下来我们可以将代码修改为launch版本的，来一次性调用多个节点。

三、修改为launch版本

新建launch文件夹，并将ld14.launch.py复制过来，此外还有ldlidar_driver和src文件夹，这些都不需要修改。

最终tree如下

lll@lll-virtual-machine:~/ros2_car_ws/src/lidar_follower$ tree
.
├── launch
│   ├── ld14.launch.py
│   └── lidar_follower.launch.py
├── ldlidar_driver
│   ├── include
│   │   ├── ldlidar_datatype.h
│   │   ├── ldlidar_driver.h
│   │   ├── lipkg.h
│   │   ├── log_module.h
│   │   ├── serial_interface_linux.h
│   │   ├── slbf.h
│   │   └── sl_transform.h
│   └── src
│       ├── ldlidar_driver.cpp
│       ├── lipkg.cpp
│       ├── log_module.cpp
│       ├── serial_interface_linux.cpp
│       ├── slbf.cpp
│       └── sl_transform.cpp
├── lidar_follower
│   ├── __init__.py
│   └── lidar_followers.py
├── package.xml
├── resource
│   └── lidar_follower
├── setup.cfg
├── setup.py
├── src
│   └── demo.cpp
└── test
    ├── test_copyright.py
    ├── test_flake8.py
    └── test_pep257.py

8 directories, 26 files

3.1 编写代码

新建lidar_follower.launch.py文件

编写代码，调用ld14.launch.py和lidar_followers

#!/usr/bin/env python3
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

  #Include LDLidar launch file
  ldlidar_launch = IncludeLaunchDescription(
      launch_description_source=PythonLaunchDescriptionSource([
          get_package_share_directory('ldlidar_sl_ros2'),
          '/launch/ld14.launch.py'
      ])
  )

  # Define LaunchDescription variable
  ld = LaunchDescription()

  ld.add_action(ldlidar_launch)

    # Add lidar_behavior node action
  ld.add_action(Node(
        package='lidar_follower',  # 替换为你的包名
        executable='lidar_follower',  # 替换为你的可执行文件名，这里的文件名时在setup.py中的entry_points注册的
        name='lidar_follower',
        output='screen',
        parameters=[
            {'target_distance': 1.5},  # 目标距离
            {'P': [1.5, 0.5]},          # PID控制器的P参数
            {'I': [0.0, 0.0]},          # PID控制器的I参数
            {'D': [0.02, 0.002]}        # PID控制器的D参数
        ]
    ))


  
  return ld

修改 package.xml

添加下面的内容

  <exec_depend>ros2launch</exec_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

setup.py保持不变

from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'lidar_follower'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    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_follower/*.py')),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.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_follower = lidar_follower.lidar_follower:main',
            'lidar_followers = lidar_follower.lidar_followers:main',
        ],
    },
)

3.2 编译运行

colcon build
source install/setup.bash
ros2 launch lidar_follower lidar_follower.launch.py

四、启动小车

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

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

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

# 切换窗口
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
colcon build
source install/setup.bash
ros2 launch lidar_follower lidar_follower.launch.py

在这里插入图片描述