CAFE

임베디드프로그래밍(11)

[ROS2] Lidar 설치

작성자한창호|작성시간26.06.08|조회수30 목록 댓글 0

* 라이다 종류

종류사양그림
SLAMTEC RPLIDAR C1

디바이스바트 99,000원		측정거리
흰색 물체：0.05~12m (70%반사율)
검정 물체：0.05~6m (10%반사율)

샘플링 주파수 5KHz
스캔 주파수 8~12Hz, 10Hz (전형적인 값)
각도 분해능 0.72°
피치 각도 0°-1.5°
통신 인터페이스 TTL UART
통신속도 460800
측정거리오차 ±30mm
거리측정 분별력 15mm
보호 등급 IP54
반사 강도 40,000lux
SLAMTEC자원 다운로드 센터 및 기술 지원

 

* 라이다 원리

구분설명
원리LiDAR란 Light Detection And Ranging (빛을 통한 검출과 거리 측정)의 약칭으로, 근적외광 및 가시광, 자외선을 사용하여 대상물에 빛을 비추고, 그 반사광을 광 센서를 통해 검출하여 거리를 측정하는 리모트 센싱

LiDAR (라이다) | 플러스 알파의 기초 지식 | TechWeb
거리계산거리는 다음과 같이 계산한다.

직접 비행시간 거리측정(dToF)
레이저 소스가 펄스화되고 각 펄스가 반사되어 센서로 돌아오는 데 걸리는 시간을 측정한다.
간접 비행시간 거리측정(iToF)
레이저 소스는 진폭 변조된다. 투과된 빛과 센서로 반사된 빛의 위상차를 측정한다. 이 위상차를 시간으로 변환한 후 빛의 속도를 사용하여 거리로 변환한다.

주파수 변조 연속파(FMCW)
레이저 소스는 CW이고 톱니 파형("Chirp")으로 변조된 주파수이다. 반사된 신호가 소스의 레퍼런스와 광학적으로 혼합되어 신호는 스펙트럼 분석에 의해 추출된, 거리에 해당하는 "비트" 주파수를 포함한다.

종류Solid-State LiDAR



MEMS LiDAR
반사 물질을 상/하 & 좌/우 반복운동 하고, N개의 Laser를 사용할 수 있다.

 

* RPLidar C1 드라이버 설치 및 테스트

구분설명
설치드라이버 사이트: https://github.com/Slamtec/rplidar_ros/tree/ros2

# 터틀봇3과 RemotePC 모두 설치한다.

$ cd ~/ros2_ws/src
$ git clone -b ros2 https://github.com/Slamtec/rplidar_ros.git

$ cd ~/ros2_ws/
$ source /opt/ros/jazzy/setup.bash
$ colcon build --symlink-install 
$ source ./install/setup.bash
실행# 터틀봇3에서 실행하기

$ ros2 launch rplidar_ros rplidar_c1_launch.py

# Remote PC에서 실행하기

$ ros2 launch rplidar_ros view_rplidar_c1_launch.py
결과결과 영상에서 Lidar가 동작하는 것을 보여준다.

Fixed Frame:  laser 또는 base_link
topic 확인# Remote PC에서 실행하기

$ ros2 topic list

$ ros2 topic echo /scan --once

다음 결과를 보면 범위가 0.15m ~ 16m 로 나온다.

---
header:
  stamp:
    sec: 1780909396
    nanosec: 651480589
  frame_id: laser
angle_min: -3.1241390705108643
angle_max: 3.1415927410125732
angle_increment: 0.008714509196579456
time_increment: 0.0001323226751992479
scan_time: 0.0951400101184845
range_min: 0.15000000596046448
range_max: 16.0
ranges:
- 0.1509999930858612
- 0.15049999952316284
- 0.15150000154972076
- 0.15150000154972076
- .inf
- 0.15074999630451202
- 0.15074999630451202
- 0.15150000154972076
- 0.15150000154972076
scan 데이터# 물체를 5cm에 갖다대고 scan 데이터를 살펴보자. 만약에 0.15 이하로 내려가지 않을 경우 다음과 같이 수정해야 한다.


* 파일 분석

파일설명
launch$ gedit ~/ros2_ws/src/rplidar_ros/launch/rplidar_c1_launch.py

#!/usr/bin/env python3

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    channel_type =  LaunchConfiguration('channel_type', default='serial')
    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
    serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800')
    frame_id = LaunchConfiguration('frame_id', default='laser')
    inverted = LaunchConfiguration('inverted', default='false')
    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
    scan_mode = LaunchConfiguration('scan_mode', default='Standard')

    return LaunchDescription([
        DeclareLaunchArgument(
            'channel_type',
            default_value=channel_type,
            description='Specifying channel type of lidar'),

        DeclareLaunchArgument(
            'serial_port',
            default_value=serial_port,
            description='Specifying usb port to connected lidar'),

        DeclareLaunchArgument(
            'serial_baudrate',
            default_value=serial_baudrate,
            description='Specifying usb port baudrate to connected lidar'),
        
        DeclareLaunchArgument(
            'frame_id',
            default_value=frame_id,
            description='Specifying frame_id of lidar'),

        DeclareLaunchArgument(
            'inverted',
            default_value=inverted,
            description='Specifying whether or not to invert scan data'),

        DeclareLaunchArgument(
            'angle_compensate',
            default_value=angle_compensate,
            description='Specifying whether or not to enable angle_compensate of scan data'),

        DeclareLaunchArgument(
            'scan_mode',
            default_value=scan_mode,
            description='Specifying scan mode of lidar'),

        Node(
            package='rplidar_ros',
            executable='rplidar_node',
            name='rplidar_node',
            parameters=[{'channel_type':channel_type,
                         'serial_port': serial_port,
                         'serial_baudrate': serial_baudrate,
                         'frame_id': frame_id,
                         'inverted': inverted,
                         'angle_compensate': angle_compensate,
                         'scan_mode': scan_mode}],
            output='screen'),
    ])

view$ gedit ~/ros2_ws/src/rplidar_ros/launch/view_rplidar_c1_launch.py


#!/usr/bin/env python3

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    channel_type =  LaunchConfiguration('channel_type', default='serial')
    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
    serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800')
    frame_id = LaunchConfiguration('frame_id', default='laser')
    inverted = LaunchConfiguration('inverted', default='false')
    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
    scan_mode = LaunchConfiguration('scan_mode', default='Standard')

    rviz_config_dir = os.path.join(
            get_package_share_directory('rplidar_ros'),
            'rviz',
            'rplidar_ros.rviz')

    return LaunchDescription([
        DeclareLaunchArgument(
            'channel_type',
            default_value=channel_type,
            description='Specifying channel type of lidar'),

        DeclareLaunchArgument(
            'serial_port',
            default_value=serial_port,
            description='Specifying usb port to connected lidar'),

        DeclareLaunchArgument(
            'serial_baudrate',
            default_value=serial_baudrate,
            description='Specifying usb port baudrate to connected lidar'),
        
        DeclareLaunchArgument(
            'frame_id',
            default_value=frame_id,
            description='Specifying frame_id of lidar'),

        DeclareLaunchArgument(
            'inverted',
            default_value=inverted,
            description='Specifying whether or not to invert scan data'),

        DeclareLaunchArgument(
            'angle_compensate',
            default_value=angle_compensate,
            description='Specifying whether or not to enable angle_compensate of scan data'),

        DeclareLaunchArgument(
            'scan_mode',
            default_value=scan_mode,
            description='Specifying scan mode of lidar'),

        Node(
            package='rplidar_ros',
            executable='rplidar_node',
            name='rplidar_node',
            parameters=[{'channel_type':channel_type,
                         'serial_port': serial_port,
                         'serial_baudrate': serial_baudrate,
                         'frame_id': frame_id,
                         'inverted': inverted,
                         'angle_compensate': angle_compensate,
                           'scan_mode': scan_mode
                         }],
            output='screen'),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            output='screen'),
    ])

* 터틀봇3에서 실행하기

구분설명
장착하기터틀봇3에 라이다를 설치한다.

Lidar 뒷 변에 네 개의 다리를 연결한다. (나사 PH_M 2.5x8mm K:  검정색 머리둥근 볼트 M2.5x8mm)

그리고 패널에 4개의 나사로 연결한다.


RPLidar C1을 UART2Usb adapter를 연결하고 USB 케이블을 라즈베리파에 연결한다.

터틀봇3
bringup 파일		# 터틀봇3에서 실행하기

$ vi ~/ros2_ws/src/turtlebot3/turtlebot3_bringup/launch/robot.launch.py

57:
    if LDS_MODEL == 'LDS-01':
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('hls_lfcd_lds_driver'), 'launch'))
    elif LDS_MODEL == 'LDS-02':
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('ld08_driver'), 'launch'))
        LDS_LAUNCH_FILE = '/ld08.launch.py'
    elif LDS_MODEL == 'LDS-03':
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('coin_d4_driver'), 'launch'))
        LDS_LAUNCH_FILE = '/single_lidar_node.launch.py'
    elif LDS_MODEL == 'RPLIDAR-C1':
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('rplidar_ros'), 'launch'))
        LDS_LAUNCH_FILE = '/rplidar_c1_launch.py'
    else:
        lidar_pkg_dir = LaunchConfiguration(
            'lidar_pkg_dir',
            default=os.path.join(get_package_share_directory('hls_lfcd_lds_driver'), 'launch'))
터틀봇3
소스 수정		# 라이다의 range 범위를 수정한다.

$ vi ~/ros2_ws/src/rplidar_ros/src/rplidar_node.cpp

256:
        scan_msg->range_min = 0.15;                         // 0.05로 수정한다.
        scan_msg->range_max = max_distance;             //8.0;

# 다시 컴파일 한다.
cd ~/ros2_ws/
colcon build --symlink-install 
터틀봇3
Model 변경		# Turtulebot3에서 실행

$ gedit ~/.bashrc

라이다 모델명을 다음과 같이 변경한다.

export LDS_MODEL=RPLIDAR-C1

$ ros2 launch turtlebot3_bringup robot.launch.py 
RemotePC
실험		# Remote PC에서 실행하기

# Driver 설치하기
cd ~/ros2_ws/src
git clone -b ros2 https://github.com/Slamtec/rplidar_ros.git
cd ~/ros2_ws/
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install 

$ ros2 launch rplidar_ros view_rplidar_c1_launch.py
토픽 확인# 토픽 /scan을 통해 라이다 범위를 확인한다.

$ ros2 topic echo /scan --once

다음 결과를 보면 범위가 0.05m ~ 16m 로 나온다.

---
header:
  stamp:
    sec: 1780909396
    nanosec: 651480589
  frame_id: laser
angle_min: -3.1241390705108643
angle_max: 3.1415927410125732
angle_increment: 0.008714509196579456
time_increment: 0.0001323226751992479
scan_time: 0.0951400101184845
range_min: 0.05000000596046448
range_max: 16.0
ranges:
- 3.9257500171661377
- 3.92549991607666
- 3.927000045776367
- 3.927000045776367
- 3.928999900817871
- 3.930000066757202
- .inf
- 3.9352500438690186
- 3.9352500438690186

* RPLidar 소스 

파일코드
view_rplidar_c1_launch.py$ vi ~/ros2_ws/src/rplidar_ros/src/launch/view_rplidar_c1_launch.py

#!/usr/bin/env python3

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    channel_type =  LaunchConfiguration('channel_type', default='serial')
    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
    serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800')
    frame_id = LaunchConfiguration('frame_id', default='laser')
    inverted = LaunchConfiguration('inverted', default='false')
    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
    scan_mode = LaunchConfiguration('scan_mode', default='Standard')

    rviz_config_dir = os.path.join(
            get_package_share_directory('rplidar_ros'),
            'rviz',
            'rplidar_ros.rviz')

    return LaunchDescription([
        DeclareLaunchArgument(
            'channel_type',
            default_value=channel_type,
            description='Specifying channel type of lidar'),

        DeclareLaunchArgument(
            'serial_port',
            default_value=serial_port,
            description='Specifying usb port to connected lidar'),

        DeclareLaunchArgument(
            'serial_baudrate',
            default_value=serial_baudrate,
            description='Specifying usb port baudrate to connected lidar'),
        
        DeclareLaunchArgument(
            'frame_id',
            default_value=frame_id,
            description='Specifying frame_id of lidar'),

        DeclareLaunchArgument(
            'inverted',
            default_value=inverted,
            description='Specifying whether or not to invert scan data'),

        DeclareLaunchArgument(
            'angle_compensate',
            default_value=angle_compensate,
            description='Specifying whether or not to enable angle_compensate of scan data'),

        DeclareLaunchArgument(
            'scan_mode',
            default_value=scan_mode,
            description='Specifying scan mode of lidar'),

        Node(
            package='rplidar_ros',
            executable='rplidar_node',
            name='rplidar_node',
            parameters=[{'channel_type':channel_type,
                         'serial_port': serial_port,
                         'serial_baudrate': serial_baudrate,
                         'frame_id': frame_id,
                         'inverted': inverted,
                         'angle_compensate': angle_compensate,
                           'scan_mode': scan_mode
                         }],
            output='screen'),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            output='screen'),
    ])
rplidar_client$ vi ~/ros2_ws/src/rplidar_ros/src/rplidar_client.cpp

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include <math.h>

#define RAD2DEG(x) ((x)*180./M_PI)

static void scanCb(sensor_msgs::msg::LaserScan::SharedPtr scan) {
  int count = scan->scan_time / scan->time_increment;
  printf("[SLLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count);
  printf("[SLLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min),
         RAD2DEG(scan->angle_max));

  for (int i = 0; i < count; i++) {
    float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
    printf("[SLLIDAR INFO]: angle-distance : [%f, %f]\n", degree, scan->ranges[i]);
  }
}

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("sllidar_client");
  auto lidar_info_sub = node->create_subscription<sensor_msgs::msg::LaserScan>(
                        "scan", rclcpp::SensorDataQoS(), scanCb);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
rplidar_node$ vi ~/ros2_ws/src/rplidar_ros/src/rplidar_node.cpp

/*  RPLIDAR ROS2 NODE
 *  http://www.slamtec.com */

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_srvs/srv/empty.hpp>
#include "sl_lidar.h"
#include "math.h"

#include <signal.h>

#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif

#define DEG2RAD(x) ((x)*M_PI/180.)

#define ROS2VERSION "1.0.1"

enum {
    LIDAR_A_SERIES_MINUM_MAJOR_ID   = 0,
    LIDAR_S_SERIES_MINUM_MAJOR_ID   = 5,
    LIDAR_T_SERIES_MINUM_MAJOR_ID   = 8,
};

using namespace sl;

bool need_exit = false;

class RPlidarNode : public rclcpp::Node
{
  public:
    RPlidarNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions())
    : Node("rplidar_node", options)
    {

      
    }

  private:    
    void init_param()
    {
        this->declare_parameter<std::string>("channel_type","serial");
        this->declare_parameter<std::string>("tcp_ip", "192.168.0.7");
        this->declare_parameter<int>("tcp_port", 20108);
        this->declare_parameter<std::string>("udp_ip","192.168.11.2");
        this->declare_parameter<int>("udp_port",8089);
        this->declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
        this->declare_parameter<int>("serial_baudrate",1000000);
        this->declare_parameter<std::string>("frame_id","laser_frame");
        this->declare_parameter<bool>("inverted", false);
        this->declare_parameter<bool>("angle_compensate", false);
        this->declare_parameter<bool>("flip_x_axis", false);
        this->declare_parameter<bool>("auto_standby", false);
        this->declare_parameter<std::string>("topic_name",std::string("scan"));
        this->declare_parameter<std::string>("scan_mode",std::string());
        this->declare_parameter<float>("scan_frequency",10);
        
        this->get_parameter_or<std::string>("channel_type", channel_type, "serial");
        this->get_parameter_or<std::string>("tcp_ip", tcp_ip, "192.168.0.7"); 
        this->get_parameter_or<int>("tcp_port", tcp_port, 20108);
        this->get_parameter_or<std::string>("udp_ip", udp_ip, "192.168.11.2"); 
        this->get_parameter_or<int>("udp_port", udp_port, 8089);
        this->get_parameter_or<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); 
        this->get_parameter_or<int>("serial_baudrate", serial_baudrate, 1000000/*256000*/);//ros run for A1 A2, change to 256000 if A3
        this->get_parameter_or<std::string>("frame_id", frame_id, "laser_frame");
        this->get_parameter_or<bool>("inverted", inverted, false);
        this->get_parameter_or<bool>("angle_compensate", angle_compensate, false);
        this->get_parameter_or<bool>("flip_x_axis", flip_x_axis, false);
        this->get_parameter_or<bool>("auto_standby", auto_standby, false);
        this->get_parameter_or<std::string>("topic_name", topic_name, "scan");
        this->get_parameter_or<std::string>("scan_mode", scan_mode, std::string());
        if(channel_type == "udp")
            this->get_parameter_or<float>("scan_frequency", scan_frequency, 20.0);
        else
            this->get_parameter_or<float>("scan_frequency", scan_frequency, 10.0);
    }

    bool getRPLIDARDeviceInfo(ILidarDriver * drv)
    {
        sl_result     op_result;
        sl_lidar_response_device_info_t devinfo;

        op_result = drv->getDeviceInfo(devinfo);
        if (SL_IS_FAIL(op_result)) {
            if (op_result == SL_RESULT_OPERATION_TIMEOUT) {
                RCLCPP_ERROR(this->get_logger(),"Error, operation time out. SL_RESULT_OPERATION_TIMEOUT! ");
            } else {
                RCLCPP_ERROR(this->get_logger(),"Error, unexpected error, code: %x",op_result);
            }
            return false;
        }

        // print out the device serial number, firmware and hardware version number..
        char sn_str[37] = {'\0'}; 
        for (int pos = 0; pos < 16 ;++pos) {
            sprintf(sn_str + (pos * 2),"%02X", devinfo.serialnum[pos]);
        }
        RCLCPP_INFO(this->get_logger(),"RPLidar S/N: %s",sn_str);
        RCLCPP_INFO(this->get_logger(),"Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF);
        RCLCPP_INFO(this->get_logger(),"Hardware Rev: %d",(int)devinfo.hardware_version);
        return true;
    }

    bool checkRPLIDARHealth(ILidarDriver * drv)
    {
        sl_result     op_result;
        sl_lidar_response_device_health_t healthinfo;
        op_result = drv->getHealth(healthinfo);
        if (SL_IS_OK(op_result)) { 
            RCLCPP_INFO(this->get_logger(),"RPLidar health status : %d", healthinfo.status);
            switch (healthinfo.status) {
                case SL_LIDAR_STATUS_OK:
                    RCLCPP_INFO(this->get_logger(),"RPLidar health status : OK.");
                    return true;
                case SL_LIDAR_STATUS_WARNING:
                    RCLCPP_INFO(this->get_logger(),"RPLidar health status : Warning.");
                    return true;
                case SL_LIDAR_STATUS_ERROR:
                    RCLCPP_ERROR(this->get_logger(),"Error, RPLidar internal error detected. Please reboot the device to retry.");
                    return false;
                default:
                    RCLCPP_ERROR(this->get_logger(),"Error, Unknown internal error detected. Please reboot the device to retry.");
                    return false;
            }
        } else {
            RCLCPP_ERROR(this->get_logger(),"Error, cannot retrieve RPLidar health code: %x", op_result);
            return false;
        }
    }

    bool stop_motor(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
                    std::shared_ptr<std_srvs::srv::Empty::Response> res)
    {
        (void)req;
        (void)res;

        if (auto_standby) {
            RCLCPP_INFO(
                this->get_logger(),
                "Ingnoring stop_motor request because rplidar_node is in 'auto standby' mode");
            return false;
        }

        RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__);
        
        //RCLCPP_DEBUG(this->get_logger(),"Stop motor");
        this->stop();
        //drv->setMotorSpeed(0);
        return true;
    }

    bool start_motor(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
                    std::shared_ptr<std_srvs::srv::Empty::Response> res)
    {
        (void)req;
        (void)res;

        if (auto_standby) {
            RCLCPP_INFO(
                this->get_logger(),
                "Ingnoring start_motor request because rplidar_node is in 'auto standby' mode");
            return false;
        }
        RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__);
        return this->start();

#if 0
        if(!drv)
           return false;
        if(drv->isConnected())
        {
            RCLCPP_DEBUG(this->get_logger(),"Start motor");
            sl_result ans=drv->setMotorSpeed();
            if (SL_IS_FAIL(ans)) {
                RCLCPP_WARN(this->get_logger(), "Failed to start motor: %08x", ans);
                return false;
            }
        
            ans=drv->startScan(0,1);
            if (SL_IS_FAIL(ans)) {
                RCLCPP_WARN(this->get_logger(), "Failed to start scan: %08x", ans);
            }
        } else {
            RCLCPP_INFO(this->get_logger(),"lost connection");
            return false;
        }

        return true;
#endif

    }

    static float getAngle(const sl_lidar_response_measurement_node_hq_t& node)
    {
        return node.angle_z_q14 * 90.f / 16384.f;
    }

    void publish_scan(rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr& pub,
                  sl_lidar_response_measurement_node_hq_t *nodes,
                  size_t node_count, rclcpp::Time start,
                  double scan_time, bool inverted, bool flip_X_axis,
                  float angle_min, float angle_max,
                  float max_distance,
                  std::string frame_id)
    {
        static int scan_count = 0;
        auto scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>();

        scan_msg->header.stamp = start;
        scan_msg->header.frame_id = frame_id;
        scan_count++;

        bool reversed = (angle_max > angle_min);
        if ( reversed ) {
            scan_msg->angle_min =  M_PI - angle_max;
            scan_msg->angle_max =  M_PI - angle_min;
        } else {
            scan_msg->angle_min =  M_PI - angle_min;
            scan_msg->angle_max =  M_PI - angle_max;
        }
        scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (double)(node_count-1);

        scan_msg->scan_time = scan_time;
        scan_msg->time_increment = scan_time / (double)(node_count-1);
        scan_msg->range_min = 0.05;
        scan_msg->range_max = max_distance;//8.0;

        scan_msg->intensities.resize(node_count);
        scan_msg->ranges.resize(node_count);
        bool reverse_data = (!inverted && reversed) || (inverted && !reversed);

        size_t scan_midpoint = node_count / 2;
        for (size_t i = 0; i < node_count; i++) {
            float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000;
            size_t apply_index = i;
            if (reverse_data) {
                apply_index = node_count - 1 - i;
            }
            if (flip_X_axis) {
                if (apply_index >= scan_midpoint)
                    apply_index = apply_index - scan_midpoint;
                else
                    apply_index = apply_index + scan_midpoint;
            }

            if (read_value == 0.0)
                scan_msg->ranges[apply_index] = std::numeric_limits<float>::infinity();
            else
                scan_msg->ranges[apply_index] = read_value;
            scan_msg->intensities[apply_index] = (float)(nodes[apply_index].quality >> 2);
        }

        pub->publish(*scan_msg);
    }

    bool set_scan_mode() {
        sl_result     op_result;
        LidarScanMode current_scan_mode;
        if (scan_mode.empty()) {
            op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, &current_scan_mode);
        }
        else {
            std::vector<LidarScanMode> allSupportedScanModes;
            op_result = drv->getAllSupportedScanModes(allSupportedScanModes);

            if (SL_IS_OK(op_result)) {
                sl_u16 selectedScanMode = sl_u16(-1);
                for (std::vector<LidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {
                    if (iter->scan_mode == scan_mode) {
                        selectedScanMode = iter->id;
                        break;
                    }
                }

                if (selectedScanMode == sl_u16(-1)) {
                    RCLCPP_ERROR(this->get_logger(), "scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str());
                    for (std::vector<LidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {
                        RCLCPP_ERROR(this->get_logger(), "\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode,
                            iter->max_distance, (1000 / iter->us_per_sample));
                    }
                    op_result = SL_RESULT_OPERATION_FAIL;
                }
                else {
                    op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, &current_scan_mode);
                }
            }
        }

        if (SL_IS_OK(op_result))
        {
            //default frequent is 10 hz (by motor pwm value),  current_scan_mode.us_per_sample is the number of scan point per us
            int points_per_circle = (int)(1000 * 1000 / current_scan_mode.us_per_sample / scan_frequency);
            angle_compensate_multiple = points_per_circle / 360.0 + 1;
            if (angle_compensate_multiple < 1)
                angle_compensate_multiple = 1.0;
            max_distance = (float)current_scan_mode.max_distance;
            RCLCPP_INFO(this->get_logger(), "current scan mode: %s, sample rate: %d Khz, max_distance: %.1f m, scan frequency:%.1f Hz, ",
                current_scan_mode.scan_mode, (int)(1000 / current_scan_mode.us_per_sample + 0.5), max_distance, scan_frequency);
            return true;
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Can not start scan: %08x!", op_result);
            return false;
        }
    }
    bool start()
    {
        if (nullptr == drv) {
            return false;
        }

        RCLCPP_INFO(this->get_logger(), "Start");
        drv->setMotorSpeed();
        if (!set_scan_mode()) {
            this->stop();
            RCLCPP_ERROR(this->get_logger(), "Failed to set scan mode");
            return false;
        }
        is_scanning = true;
        return true;
    }

    void stop()
    {
        if (nullptr == drv) {
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Stop");
        drv->stop();
        drv->setMotorSpeed(0);
        is_scanning = false;
    }

public:    
    int work_loop()
    {        
        init_param();
        int ver_major = SL_LIDAR_SDK_VERSION_MAJOR;
        int ver_minor = SL_LIDAR_SDK_VERSION_MINOR;
        int ver_patch = SL_LIDAR_SDK_VERSION_PATCH;
        RCLCPP_INFO(this->get_logger(),"RPLidar running on ROS2 package rplidar_ros. RPLIDAR SDK Version:%d.%d.%d",ver_major,ver_minor,ver_patch);
    
        sl_result     op_result;
        // create the driver instance
        drv = *createLidarDriver();
        if (nullptr == drv) {
            /* don't start spinning without a driver object */
            RCLCPP_ERROR(this->get_logger(), "Failed to construct driver");
            return -1;
        }
        IChannel* _channel;
        if(channel_type == "tcp"){
            _channel = *createTcpChannel(tcp_ip, tcp_port);
        }
        else if(channel_type == "udp"){
            _channel = *createUdpChannel(udp_ip, udp_port);
        }
        else{
            _channel = *createSerialPortChannel(serial_port, serial_baudrate);
        }
        if (SL_IS_FAIL((drv)->connect(_channel))) {
            if(channel_type == "tcp"){
                RCLCPP_ERROR(this->get_logger(),"Error, cannot connect to the ip addr  %s with the tcp port %s.",tcp_ip.c_str(),std::to_string(tcp_port).c_str());
            }
            else if(channel_type == "udp"){
                RCLCPP_ERROR(this->get_logger(),"Error, cannot connect to the ip addr  %s with the udp port %s.",udp_ip.c_str(),std::to_string(udp_port).c_str());
            }
            else{
                RCLCPP_ERROR(this->get_logger(),"Error, cannot bind to the specified serial port %s.",serial_port.c_str());            
            }
            delete drv; drv = nullptr;
            return -1;
        }
        
        // get rplidar device info
        if (!getRPLIDARDeviceInfo(drv)) {
            delete drv; drv = nullptr;
            return -1;
        }

        // check health...
        if (!checkRPLIDARHealth(drv)) {
            delete drv; drv = nullptr;
            return -1;
        }

        sl_lidar_response_device_info_t devinfo;
        op_result = drv->getDeviceInfo(devinfo);
        bool scan_frequency_tunning_after_scan = false;

        if( (devinfo.model>>4) > LIDAR_S_SERIES_MINUM_MAJOR_ID){
            scan_frequency_tunning_after_scan = true;
        }

        if(!scan_frequency_tunning_after_scan){ //for RPLIDAR A serials
            //start RPLIDAR A serials  rotate by pwm
            drv->setMotorSpeed(600);
        }

        /* start motor and scanning */
        if (!auto_standby && !this->start()) {
            delete drv; drv = nullptr;
            return -1;
        }

        scan_pub = this->create_publisher<sensor_msgs::msg::LaserScan>(topic_name, rclcpp::QoS(rclcpp::KeepLast(10)));

        stop_motor_service = this->create_service<std_srvs::srv::Empty>("stop_motor",  
                                std::bind(&RPlidarNode::stop_motor,this,std::placeholders::_1,std::placeholders::_2));
        start_motor_service = this->create_service<std_srvs::srv::Empty>("start_motor", 
                                std::bind(&RPlidarNode::start_motor,this,std::placeholders::_1,std::placeholders::_2));

        //drv->setMotorSpeed();

        rclcpp::Time start_scan_time;
        rclcpp::Time end_scan_time;
        double scan_duration;
        while (rclcpp::ok() && !need_exit) {
            sl_lidar_response_measurement_node_hq_t nodes[8192];
            size_t   count = _countof(nodes);

            if (auto_standby) {
                if (scan_pub->get_subscription_count() > 0 && !is_scanning) {
                    this->start();
                }
                else if (scan_pub->get_subscription_count() == 0) {
                    if (is_scanning) {
                        this->stop();
                    }
                }
            }

            start_scan_time = this->now();
            op_result = drv->grabScanDataHq(nodes, count);
            end_scan_time = this->now();
            scan_duration = (end_scan_time - start_scan_time).seconds();

            if (op_result == SL_RESULT_OK) {
                if(scan_frequency_tunning_after_scan) { //Set scan frequency(For Slamtec Tof lidar)
                    RCLCPP_INFO(this->get_logger(), "set lidar scan frequency to %.1f Hz(%.1f Rpm) ",scan_frequency,scan_frequency*60);
                    drv->setMotorSpeed(scan_frequency*60); //rpm 
                    scan_frequency_tunning_after_scan = false;
                    continue;
                }
                op_result = drv->ascendScanData(nodes, count);
                float angle_min = DEG2RAD(0.0f);
                float angle_max = DEG2RAD(359.0f);
                if (op_result == SL_RESULT_OK) {
                    if (angle_compensate) {
                        //const int angle_compensate_multiple = 1;
                        const int angle_compensate_nodes_count = 360*angle_compensate_multiple;
                        int angle_compensate_offset = 0;
                        auto angle_compensate_nodes = new sl_lidar_response_measurement_node_hq_t[angle_compensate_nodes_count];
                        memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(sl_lidar_response_measurement_node_hq_t));

                        size_t i = 0, j = 0;
                        for( ; i < count; i++ ) {
                            if (nodes[i].dist_mm_q2 != 0) {
                                float angle = getAngle(nodes[i]);
                                int angle_value = (int)(angle * angle_compensate_multiple);
                                if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
                                for (j = 0; j < angle_compensate_multiple; j++) {
                                    int angle_compensate_nodes_index = angle_value-angle_compensate_offset + j;
                                    if(angle_compensate_nodes_index >= angle_compensate_nodes_count)
                                        angle_compensate_nodes_index = angle_compensate_nodes_count - 1;
                                    angle_compensate_nodes[angle_compensate_nodes_index] = nodes[i];
                                }
                            }
                        }
    
                        publish_scan(scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
                                start_scan_time, scan_duration, inverted, flip_x_axis,
                                angle_min, angle_max, max_distance,
                                frame_id);

                        if (angle_compensate_nodes) {
                            delete[] angle_compensate_nodes;
                            angle_compensate_nodes = nullptr;
                        }
                    } else {
                        int start_node = 0, end_node = 0;
                        int i = 0;
                        // find the first valid node and last valid node
                        while (nodes[i++].dist_mm_q2 == 0);
                        start_node = i-1;
                        i = count -1;
                        while (nodes[i--].dist_mm_q2 == 0);
                        end_node = i+1;

                        angle_min = DEG2RAD(getAngle(nodes[start_node]));
                        angle_max = DEG2RAD(getAngle(nodes[end_node]));

                        publish_scan(scan_pub, &nodes[start_node], end_node-start_node +1,
                                start_scan_time, scan_duration, inverted, flip_x_axis, 
                                angle_min, angle_max, max_distance,
                                frame_id);
                    }
                } else if (op_result == SL_RESULT_OPERATION_FAIL) {
                    // All the data is invalid, just publish them
                    float angle_min = DEG2RAD(0.0f);
                    float angle_max = DEG2RAD(359.0f);
                    publish_scan(scan_pub, nodes, count,
                                start_scan_time, scan_duration, inverted, flip_x_axis,
                                angle_min, angle_max, max_distance,
                                frame_id);
                }
            }

            rclcpp::spin_some(shared_from_this());
        }

        // done!
        drv->setMotorSpeed(0);
        drv->stop();
        RCLCPP_INFO(this->get_logger(),"Stop motor");
        if (drv) { delete drv;  drv = nullptr; }
        return 0;
    }

  private:
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr start_motor_service;
    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_motor_service;

    std::string channel_type;
    std::string tcp_ip;
    std::string udp_ip;
    std::string serial_port;
    std::string topic_name;
    int tcp_port = 20108;
    int udp_port = 8089;
    int serial_baudrate = 115200;
    std::string frame_id;
    bool inverted = false;
    bool angle_compensate = true;
    bool flip_x_axis = false;
    bool auto_standby = false;
    float max_distance = 8.0;
    size_t angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree
    std::string scan_mode;
    float scan_frequency;
    /* State */
    bool is_scanning = false;

    ILidarDriver *drv = nullptr;
};

void ExitHandler(int sig)
{
    (void)sig;
    need_exit = true;
}


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);  
  auto rplidar_node = std::make_shared<RPlidarNode>(rclcpp::NodeOptions());
  signal(SIGINT,ExitHandler);
  int ret = rplidar_node->work_loop();
  rclcpp::shutdown();
  return ret;
}

 

다음검색
현재 게시글 추가 기능 열기

댓글

댓글 리스트
맨위로

카페 검색

카페 검색어 입력폼