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

项目四：激光雷达环境监控

结合激光雷达与摄像头，实现小车移动过程中的环境监控与拍照

在 ROS 2 中，使用激光雷达进行环境监控是一个常见且有效的应用。环境监控涉及到实时感知和分析周围环境的变化，以便做出相应的响应或决策。在自动驾驶汽车中，雷达环境检测可以实时监测周围环境，检测障碍物和其他车辆的移动，从而避免碰撞，提高行车安全性。 在工业自动化和仓库管理中，雷达检测可以防止机器人与人或物体发生碰撞，确保操作的安全性。与其他传感器（如摄像头、红外传感器）结合使用，雷达可以提供更加全面和精确的环境感知信息。

这节我们就结合雷达和摄像头来共同完成，做到移动监控拍照功能。

一、实现激光雷达环境监控

1.1 实现步骤：

  • 构建一个新的功能包，包名叫做lidar_monitor
  • 在软件包中新建一个节点，节点名为lidar_monitor.py
  • 在节点中，申请订阅话题/sacn，并设置回调函数为lidar_callback()
  • 构建回调函数lidar_callback()，用来接收和处理雷达数据
  • 显示雷达检测到的前方障碍物距离
  • 如果存在前一次扫描数据self.previous_scan，计算当前与前一次扫描数据的时间差time_diff。在时间差小于1秒的情况下，计算距离差distance_diff，检查是否有物体移动超过20厘米，记录移动方向和距离
  • 如果雷达发布移动次数1s超过10次，则发布速度指令，控制机器人移动
  • 调用相机拍照并保存图像

1.2 编写代码

构建一个新的功能包lidar_monitor

ros2 pkg create --build-type ament_python lidar_monitor

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

lidar_monitor.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
import time
import os
from datetime import datetime
from pathlib import Path
import cv2  # OpenCV库

class LidarBehavior(Node):
    def __init__(self):
        super().__init__('lidar_monitor')
        self.vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.lidar_sub = self.create_subscription(LaserScan, 'scan', self.lidar_callback, 10)
        self.previous_scan = None
        self.previous_time = None
        self.angle_tolerance = np.deg2rad(10)  # 角度容忍度设置为2度
        self.movement_count = 0
        self.capture_time = None
        self.moving_to_position = False
        self.capture_path = Path('/home/lll/capture')
        self.ensure_capture_path()

    def ensure_capture_path(self):
        if not self.capture_path.exists():
            self.capture_path.mkdir(parents=True)
            self.get_logger().info(f"创建目录: {self.capture_path}")

    def lidar_callback(self, msg):
        if self.moving_to_position:
            return  # 如果小车正在移动到指定位置，则跳过处理

        # 获取当前时间
        current_time = time.time()
        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 self.previous_scan is not None and self.previous_time is not None:
            time_diff = current_time - self.previous_time
            if time_diff <= 1.0:
                prev_distances = self.previous_scan[front_indices]
                prev_angles = self.previous_angles[front_indices]
                distance_diff = front_distances - prev_distances

                # 检查是否有物体移动超过20厘米，并加上一个误差容忍度
                distance_tolerance = 0.05  # 1厘米的误差容忍度
                moved_indices = np.where(np.abs(distance_diff) > (0.2 + distance_tolerance))[0]
                if len(moved_indices) > 0:
                    for index in moved_indices:
                        movement_distance = distance_diff[index]
                        movement_angle = front_angles[index]
                        previous_angle = prev_angles[index]
                        if np.abs(movement_angle - previous_angle) <= self.angle_tolerance:
                            movement_direction = "靠近" if movement_distance < 0 else "远离"
                            self.get_logger().info(f"检测到物体移动：方向 = {movement_direction}, 角度 = {np.rad2deg(movement_angle)} 度, 移动距离 = {np.abs(movement_distance)} 米")
                            self.movement_count += 1

        # 更新前一次扫描数据和时间
        self.previous_scan = ranges
        self.previous_angles = angles
        self.previous_time = current_time

        # 检查是否需要移动小车到指定位置并拍照
        if self.movement_count >= 10:
            self.get_logger().info("检测到多次移动，移动小车到指定位置并调用相机拍照")
            self.move_to_position_and_capture()
            self.movement_count = 0  # 重置计数器

        # 计算距离最近的目标位置
        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 move_to_position_and_capture(self):
        self.moving_to_position = True
        self.get_logger().info("小车移动到指定位置")
        # 在这里添加移动小车到指定位置的代码，例如通过发布特定的速度指令
        # 这里假设小车移动需要3秒钟
        time.sleep(3)
        self.get_logger().info("小车已到达指定位置，调用相机拍照")
        self.capture_image()
        self.moving_to_position = False

    def capture_image(self):
        cap = cv2.VideoCapture(0)  # 打开默认的相机
        if not cap.isOpened():
            self.get_logger().error("无法打开相机")
            return

        ret, frame = cap.read()
        if ret:
            image_name = self.capture_path / f"capture_{datetime.now().strftime('%Y%m%d_%H%M%S')}.jpg"
            cv2.imwrite(str(image_name), frame)  # 保存图像
            self.get_logger().info(f"调用相机拍照，图像保存路径: {image_name}")
        else:
            self.get_logger().error("无法获取相机帧")

        cap.release()

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_monitor'

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, 'lidar_monitor'), glob('lidar_monitor/*.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_monitor = lidar_monitor.lidar_monitor:main',
        ],
    },
)

1.3 launch运行

还像上一节中一样将雷达启动节点写在一起就可以了

终端一：

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

终端二：

sudo chmod 666 /dev/video0  # 先给摄像头权限
colcon build
source install/setup.bash
ros2 launch lidar_monitor lidar_monitor.launch.py

1.4 拍照结果

最终会自动创建captrue文件夹用来放置图片 在这里插入图片描述