OrangePi 5：ROS2 Humble中使用激光雷达

OrangePi 5：ROS2 Humble中使用激光雷达

本文将详细介绍如何在OrangePi5开发板中搭建ROS2开发环境，并在ROS2环境中使用激光雷达。

1、硬件准备

本次实例将使用到如下硬件模块：

• OrangePi5开发板（OrangePi5 B或OrangePi 5 Plus）（如果是OrangePi 5开发板，还需要WiFi模块）
• YDLIDAR X2激光雷达

YDLIDAR X2激光雷达通过USB接口接入OrangePi 5开发板。

2、ROS2 Humble安装

本次实例将使用ROS2的版本为Humble，该版本为ROS2的长期版本，在Ubuntu 22.04中运行良好。在这时，我们在OrangePi 5开发板中使用的操作为官方提供的Ubuntu操作系统，该操作操作系统支持NPU、GPU，因此在使用ROS2的可视化时，能够进行硬件加速。

请下载桌面版本镜像安装。

2.1 使用集成脚本安装

在OrangePi5中，安装ROS2有两种方式，一种是OrangePi 5官方提供的ROS2安装脚本，另外一种是参考ROS2官方提供的安装方法。

OrangePi5官方提供的ROS2安装脚本，只需要在命令行终端中运行如下命令即可自动完成安装即可：

orangepi@orangepi:~$ install_ros.sh ros2

在完成安装后，可以通过运行

ros2 -h

来判断是否完成安装。

2.2 按ROS2官方指导安装

第一步，配置操作系统的Local，包含语言等，只需要在命令行中，执行如下命令即可：

locale  # check for UTF-8

sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

locale  # 验证设置

第二步，添加ROS2安装源

sudo apt install software-properties-common
sudo add-apt-repository universe

添加ROS2 GPG密钥

sudo apt update && sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg

请注意，githubusercontent在国内可能下载不了，可以在这里下载：

• https://gitee.com/vision-intelligence/iot-course-code-pub/blob/master/ros.key

下载完成后，执行命令复制

sudo cp ros.key /usr/share/keyrings/ros-archive-keyring.gpg

添加ROS2安装源到source列表中

echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

第三步，安装ROS2相关包

sudo apt-get update

在上面命令完成后，建议更新Ubuntu操作系统。可以执行如下命令：

sudo apt-get upgrade

接着，执行如下命令，安装ROS2

sudo apt install ros-humble-desktop

如果需要完整安装，请执行：

sudo apt install ros-humble-desktop-full

最后，安装ROS工具

sudo apt install ros-dev-tools

2.3 ROS2安装验证

1）环境配置，执行如下命令将ROS的环境添加到当前Shell中，

source /opt/ros/humble/setup.sh

或者

source /opt/ros/humble/setup.bash

2）运行简单示例。在这里，我们运行Talker-Listener示例，Talker负责发布消息，Listener负责订阅和接收消息。

启动Talker：

source /opt/ros/humble/setup.sh
ros2 run demo_nodes_cpp talker

启动Listener：

source /opt/ros/humble/setup.bash
ros2 run demo_nodes_py listener

我们将看到如下结果：

3、YDLIDAR X2激光雷达驱动安装

3.1 YDLIDAR X2激光雷达介绍

YDLIDAR X2 激光雷达是深圳玩智商科技有限公司（EAI）研发的一款 360 度二维测距产 品（以下简称：X2）。本产品基于三角测距原理，并配以相关光学、电学、算法设计，实现 高频高精度的距离测量，在测距的同时，机械结构 360 度旋转，不断获取角度信息，从而实 现了 360 度扫描测距，输出扫描环境的点云数据。

YDLIDAR X2激光雷达具有如下特性：

• 360 度全方位扫描测距
• 测距误差小，测距稳定性好，精度高
• 测距范围广
• 抗环境光干扰能力强
• 功耗低，体积小，性能稳定，寿命长
• 激光功率满足 Class I 级别的激光器安全标准
• 电机转速可调，建议使用转速 6Hz
• 测距频率可达 3kHz

3.2 YDLIDAR X2激光雷达SDK安装

首先，下载YDLIDAR X2激光雷达SDK：

git clone https://github.com/YDLIDAR/YDLidar-SDK

接着，编译并安装YDLIDAR X2激光雷达SDK

cd YDLidar-SDK
mkdir build
cd build
cmake ..
make -j8 && sudo make install

3.3 YDLIDAR X2激光雷达驱动修改与安装

首先，创建一个ROS2工作空间目录

mkdir -p ydlidar_ros2_ws/src

接着下载，YDLIDAR X2激光雷达的ROS2驱动：

git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver

请注意，当前YDLIDAR提供的ROS2驱动，对ROS2的Humble版本API并不兼容，需要对ydlidar_ros2_driver_node.cpp这个文件进行修改。修改结果如下：

/*
 *  YDLIDAR SYSTEM
 *  YDLIDAR ROS 2 Node
 *
 *  Copyright 2017 - 2020 EAI TEAM
 *  http://www.eaibot.com
 *
 */

#ifdef _MSC_VER
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#endif

#include "src/CYdLidar.h"
#include <math.h>
#include <chrono>
#include <iostream>
#include <memory>

#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time_source.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
#include <vector>
#include <iostream>
#include <string>
#include <signal.h>

#define ROS2Verision "1.0.1"


int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_node");

  RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Current ROS Driver Version: %s\n", ((std::string)ROS2Verision).c_str());

  CYdLidar laser;
  std::string str_optvalue = "/dev/ydlidar";
  node->declare_parameter("port",str_optvalue);
  node->get_parameter("port", str_optvalue);
  ///lidar port
  laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size());

  ///ignore array
  str_optvalue = "";
  node->declare_parameter("ignore_array",str_optvalue);
  node->get_parameter("ignore_array", str_optvalue);
  laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size());

  std::string frame_id = "laser_frame";
  node->declare_parameter("frame_id", frame_id);
  node->get_parameter("frame_id", frame_id);

  //int property/
  /// lidar baudrate
  int optval = 230400;
  node->declare_parameter("baudrate",optval);
  node->get_parameter("baudrate", optval);
  laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int));
  /// tof lidar
  optval = TYPE_TRIANGLE;
  node->declare_parameter("lidar_type",optval);
  node->get_parameter("lidar_type", optval);
  laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
  /// device type
  optval = YDLIDAR_TYPE_SERIAL;
  node->declare_parameter("device_type",optval);
  node->get_parameter("device_type", optval);
  laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
  /// sample rate
  optval = 9;
  node->declare_parameter("sample_rate",optval);
  node->get_parameter("sample_rate", optval);
  laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
  /// abnormal count
  optval = 4;
  node->declare_parameter("abnormal_check_count",optval);
  node->get_parameter("abnormal_check_count", optval);
  laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));

  /// Intenstiy bit count
  optval = 8;
  node->declare_parameter("intensity_bit", optval);
  node->get_parameter("intensity_bit", optval);
  laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
     
  //bool property/
  /// fixed angle resolution
  bool b_optvalue = false;
  node->declare_parameter("fixed_resolution",b_optvalue);
  node->get_parameter("fixed_resolution", b_optvalue);
  laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
  /// rotate 180
  b_optvalue = true;
  node->declare_parameter("reversion",b_optvalue);
  node->get_parameter("reversion", b_optvalue);
  laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
  /// Counterclockwise
  b_optvalue = true;
  node->declare_parameter("inverted",b_optvalue);
  node->get_parameter("inverted", b_optvalue);
  laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
  b_optvalue = true;
  node->declare_parameter("auto_reconnect",b_optvalue);
  node->get_parameter("auto_reconnect", b_optvalue);
  laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
  /// one-way communication
  b_optvalue = false;
  node->declare_parameter("isSingleChannel",b_optvalue);
  node->get_parameter("isSingleChannel", b_optvalue);
  laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
  /// intensity
  b_optvalue = false;
  node->declare_parameter("intensity",b_optvalue);
  node->get_parameter("intensity", b_optvalue);
  laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
  /// Motor DTR
  b_optvalue = false;
  node->declare_parameter("support_motor_dtr",b_optvalue);
  node->get_parameter("support_motor_dtr", b_optvalue);
  laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));

  //float property/
  /// unit: °
  float f_optvalue = 180.0f;
  node->declare_parameter("angle_max",f_optvalue);
  node->get_parameter("angle_max", f_optvalue);
  laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
  f_optvalue = -180.0f;
  node->declare_parameter("angle_min",f_optvalue);
  node->get_parameter("angle_min", f_optvalue);
  laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
  /// unit: m
  f_optvalue = 64.f;
  node->declare_parameter("range_max",f_optvalue);
  node->get_parameter("range_max", f_optvalue);
  laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
  f_optvalue = 0.1f;
  node->declare_parameter("range_min",f_optvalue);
  node->get_parameter("range_min", f_optvalue);
  laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
  /// unit: Hz
  f_optvalue = 10.f;
  node->declare_parameter("frequency",f_optvalue);
  node->get_parameter("frequency", f_optvalue);
  laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float));

  bool invalid_range_is_inf = false;
  node->declare_parameter("invalid_range_is_inf",invalid_range_is_inf);
  node->get_parameter("invalid_range_is_inf", invalid_range_is_inf);


  bool ret = laser.initialize();
  if (ret) {
    ret = laser.turnOn();
  } else {
    RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError());
  }
  
  auto laser_pub = node->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS());

  auto stop_scan_service =
    [&laser](const std::shared_ptr<rmw_request_id_t> request_header,
  const std::shared_ptr<std_srvs::srv::Empty::Request> req,
  std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
  {
    return laser.turnOff();
  };

  auto stop_service = node->create_service<std_srvs::srv::Empty>("stop_scan",stop_scan_service);

  auto start_scan_service =
    [&laser](const std::shared_ptr<rmw_request_id_t> request_header,
  const std::shared_ptr<std_srvs::srv::Empty::Request> req,
  std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
  {
    return laser.turnOn();
  };

  auto start_service = node->create_service<std_srvs::srv::Empty>("start_scan",start_scan_service);

  rclcpp::WallRate loop_rate(20);

  while (ret && rclcpp::ok()) {

    LaserScan scan;//

    if (laser.doProcessSimple(scan)) {

      auto scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>();

      scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp);
      scan_msg->header.stamp.nanosec =  scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec);
      scan_msg->header.frame_id = frame_id;
      scan_msg->angle_min = scan.config.min_angle;
      scan_msg->angle_max = scan.config.max_angle;
      scan_msg->angle_increment = scan.config.angle_increment;
      scan_msg->scan_time = scan.config.scan_time;
      scan_msg->time_increment = scan.config.time_increment;
      scan_msg->range_min = scan.config.min_range;
      scan_msg->range_max = scan.config.max_range;
      
      int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1;
      scan_msg->ranges.resize(size);
      scan_msg->intensities.resize(size);
      for(size_t i=0; i < scan.points.size(); i++) {
        int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment);
        if(index >=0 && index < size) {
          scan_msg->ranges[index] = scan.points[i].range;
          scan_msg->intensities[index] = scan.points[i].intensity;
        }
      }

      laser_pub->publish(*scan_msg);


    } else {
      RCLCPP_ERROR(node->get_logger(), "Failed to get scan");
    }
    if(!rclcpp::ok()) {
      break;
    }
    rclcpp::spin_some(node);
    loop_rate.sleep();
  }


  RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping .......");
  laser.turnOff();
  laser.disconnecting();
  rclcpp::shutdown();

  return 0;

}

主要修改declare_parameter调用方式。在Humble版本中，C++客户端API中Node类的declare_parameter需要显示指定默认参数。

最后，在命令行中选择如下命令完成驱动编译

cd ydlidar_ros2_ws
colcon build --symlink-install

4、YDLIDAR X2激光雷达ROS2驱动验证

在完成YDLIDAR X2激光雷达的SDK和ROS2驱动之后，接下来的工具是验证YDLIDAR X2激光雷达在ROS2环境中是否能够正常工作。

第一步，安装USB设备识别。在ydlidar_ros2_ws目录中，运行如下命令：

sudo sh src/ydlidar_ros2_driver/startup/initenv.sh

第二步，配置环境，在中运行如下命令：

source /opt/ros/humble/setup.sh
source install/settup.sh

第三步，启动YDLIDAR X2激光雷达服务节点

请注意，ydlidar_ros2_driver/launch的ydlidar_launch.py、ydlidar_launch_view.py与ROS2 Humble版本不兼容，需要修改。

ydlidar_launch.py文件修改后的内容如下：

#!/usr/bin/python3
# Copyright 2020, EAIBOT
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import LogInfo

import lifecycle_msgs.msg
import os


def generate_launch_description():
    share_dir = get_package_share_directory('ydlidar_ros2_driver')
    parameter_file = LaunchConfiguration('params_file')
    node_name = 'ydlidar_ros2_driver_node'

    params_declare = DeclareLaunchArgument('params_file',
                                           default_value=os.path.join(
                                               share_dir, 'params', 'X2.yaml'),
                                           description='FPath to the ROS2 parameters file to use.')

    driver_node = LifecycleNode(package='ydlidar_ros2_driver',
                                executable='ydlidar_ros2_driver_node',
                                name='ydlidar_ros2_driver_node',
                                output='screen',
                                emulate_tty=True,
                                parameters=[parameter_file],
                                namespace='/',
                                )
    tf2_node = Node(package='tf2_ros',
                    executable='static_transform_publisher',
                    name='static_tf_pub_laser',
                    arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'],
                    )

    return LaunchDescription([
        params_declare,
        driver_node,
        tf2_node,
    ])

由于我们使用的是X2型号激光雷达，因此需要使用X2的参数文件：

 params_declare = DeclareLaunchArgument('params_file',
                                           default_value=os.path.join(
                                               share_dir, 'params', 'X2.yaml'),
                                           description='FPath to the ROS2 parameters file to use.')

ydlidar_launch_view.py文件修改后的内容如下：

#!/usr/bin/python3
# Copyright 2020, EAIBOT
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import LogInfo

import lifecycle_msgs.msg
import os


def generate_launch_description():
    share_dir = get_package_share_directory('ydlidar_ros2_driver')
    rviz_config_file = os.path.join(share_dir, 'config','ydlidar.rviz')
    parameter_file = LaunchConfiguration('params_file')
    node_name = 'ydlidar_ros2_driver_node'

    params_declare = DeclareLaunchArgument('params_file',
                                           default_value=os.path.join(
                                               share_dir, 'params', 'ydlidar.yaml'),
                                           description='FPath to the ROS2 parameters file to use.')

    driver_node = LifecycleNode(package='ydlidar_ros2_driver',
                                executable='ydlidar_ros2_driver_node',
                                name='ydlidar_ros2_driver_node',
                                output='screen',
                                emulate_tty=True,
                                parameters=[parameter_file],
                                namespace='/',
                                )
    tf2_node = Node(package='tf2_ros',
                    executable='static_transform_publisher',
                    name='static_tf_pub_laser',
                    arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'],
                    )
    rviz2_node = Node(package='rviz2',
                    executable='rviz2',
                    name='rviz2',
                    arguments=['-d', rviz_config_file],
                    )

    return LaunchDescription([
        params_declare,
        driver_node,
        tf2_node,
        rviz2_node,
    ])

接着来，在命令行终端中分别启动launch和launch_view这两个服务：

1）启动ydlidar_launch.py

source /opt/ros/humble/setup.sh
source install/settup.sh
ros2 launch ydlidar_ros2_driver ydlidar_launch.py 

2）启动ydlidar_launch_view.py

source /opt/ros/humble/setup.sh
source install/settup.sh
ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py 

可以看到，YDLIDAR X2激光雷达已经成功整合到ROS2中。