ros2激光雷达功能包移植（思岚rplidar A1）

#最新版功能包已经完善，加入参数及launch文件和stop_motor等服务，但手上并没有思岚A1雷达可以测试
#最新代码在我github上如有哪位老哥测试可以用请告知我一下来更新博客2020-04-05

目前来说还未找到思岚官方提供的ros2功能包，周末没事的时候移植了一下，用ROSTOGO试了一下，可以用（测试环境Ubuntu18.04+ROS2Dashing）。
根据官方提供的ros1功能包来看，只需要把封装好的sdk放在我们ros2功能包下即可。
首先我们先创建功能包rplidar_ros

ros2 pkg create rplidar_ros --build-type ament_cmake --dependencies std_msgs rclcpp std_srvs sensor_msgs

然后将官方功能包sdk文件夹放在我们的功能包文件夹下。
进入src文件夹创建node_A1.cpp，代码如下：

//2020.02.29
//rplidar-A1

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.h"
#include "rplidar.h"
#include 
#include 
#include 
#include 

#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif
#define M_PI 3.1415926
#define DEG2RAD(x) ((x)*M_PI/180.)

int output_angle_min = 0;
int output_angle_max = 0;
rclcpp::Node::SharedPtr node_handle = nullptr;

using namespace rp::standalone::rplidar;

RPlidarDriver * drv = NULL;

rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;//rel_vel
void publish_scan(rplidar_response_measurement_node_hq_t *nodes,
                  size_t node_count, rclcpp::Time start,
                  double scan_time, bool inverted,
                  float angle_min, float angle_max,
                  float max_distance,
                  std::string frame_id)
{
    static int scan_count = 0;
    sensor_msgs::msg::LaserScan scan_msg;

    scan_msg.header.stamp = start;
    scan_msg.header.frame_id = frame_id;
    scan_count++;
    scan_msg.angle_min = DEG2RAD(output_angle_min);
    scan_msg.angle_max = DEG2RAD(output_angle_max);
    bool reversed = (angle_max > angle_min);
    scan_msg.angle_increment = DEG2RAD(1);
    scan_msg.scan_time = scan_time;
    scan_msg.time_increment = scan_time / (double)(node_count-1);
    scan_msg.range_min = 0.15;
    scan_msg.range_max = max_distance;//8.0;

    std::vector<float> ranges, intensities;
    ranges.resize(node_count);
    intensities.resize(node_count);
    if(output_angle_max < 0 || output_angle_max > 180)RCLCPP_ERROR(node_handle->get_logger(),"output_angle_max invalid");
    if(output_angle_min < -180 || output_angle_min > 0)RCLCPP_ERROR(node_handle->get_logger(),"out_angle_min invalid");

    bool reverse_data = (!inverted && reversed) || (inverted && !reversed);

    for (size_t i = 0; i < node_count; i++) {
        float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000;
        if (read_value == 0.0)
            ranges[i] = std::numeric_limits<float>::infinity();
        else
            ranges[i] = read_value;
        intensities[i] = (float) (nodes[i].quality >> 2);
    }

    for(int i=270-output_angle_max; i<270; i++) {
        scan_msg.ranges.push_back(ranges[i]);
        scan_msg.intensities.push_back(intensities[i]);            
    }	
    RCLCPP_DEBUG(node_handle->get_logger(),"2 scan_msg.ranges.size(): %d", scan_msg.ranges.size());

    if(output_angle_min > -90) {
	//270~359
        for(int i=270; i<270-output_angle_min; i++) {
            scan_msg.ranges.push_back(ranges[i]);
            scan_msg.intensities.push_back(intensities[i]);
        }
     	
    } else {
        for(int i=270; i<359; i++) {
            scan_msg.ranges.push_back(ranges[i]);
            scan_msg.intensities.push_back(intensities[i]);
        }
        
        for(int i=0; i<=270-output_angle_min-360; i++) {
            scan_msg.ranges.push_back(ranges[i]);
            scan_msg.intensities.push_back(intensities[i]);
        }
    }
    RCLCPP_DEBUG(node_handle->get_logger(),"1 scan_msg.ranges.size(): %d", scan_msg.ranges.size());

    if(reverse_data) {
        std::reverse(scan_msg.ranges.begin(), scan_msg.ranges.end());
        std::reverse(scan_msg.intensities.begin(), scan_msg.intensities.end());
    }
    scan_pub->publish(scan_msg);
}


bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
{
    u_result     op_result;
    rplidar_response_device_info_t devinfo;

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

    // print out the device serial number, firmware and hardware version number..
    printf("RPLIDAR S/N: ");
    for (int pos = 0; pos < 16 ; ++pos) {
        printf("%02X", devinfo.serialnum[pos]);
    }
    printf("\n");
    RCLCPP_INFO(node_handle->get_logger(),"Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF);
    RCLCPP_INFO(node_handle->get_logger(),"Hardware Rev: %d",(int)devinfo.hardware_version);
    return true;
}

bool checkRPLIDARHealth(RPlidarDriver * drv)
{
    u_result     op_result;
    rplidar_response_device_health_t healthinfo;

    op_result = drv->getHealth(healthinfo);
    if (IS_OK(op_result)) {
        RCLCPP_INFO(node_handle->get_logger(),"RPLidar health status : %d", healthinfo.status);
        if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
            RCLCPP_ERROR(node_handle->get_logger(),"Error, rplidar internal error detected. Please reboot the device to retry.");
            return false;
        } else {
            return true;
        }

    } else {
        RCLCPP_ERROR(node_handle->get_logger(),"Error, cannot retrieve rplidar health code: %x", op_result);
        return false;
    }
}

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



int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
	node_handle = rclcpp::Node::make_shared("rplidar_node");

    std::string serial_port;
    int serial_baudrate = 115200;
    std::string frame_id;
    bool inverted = false;
    bool angle_compensate = true;
    float max_distance = 8.0;
    int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree

    //以下读取参数，暂时未实现
	// auto node_param = rclcpp::Node::make_shared("set_rplidar_node");
    // node_param->declare_parameter("serial_port");
    // node_param->declare_parameter("serial_baudrate");
    // node_param->declare_parameter("frame_id");
    // node_param->declare_parameter("inverted");
    // node_param->declare_parameter("angle_compensate");
    // node_param->declare_parameter("scan_mode");
    // node_param->declare_parameter("output_angle_min");
    // node_param->declare_parameter("output_angle_max");
    // auto parameters_client = std::make_shared(node_param);
    // while (!parameters_client->wait_for_service(1s)) {
    //     if (!rclcpp::ok()) {
    //     RCLCPP_ERROR(node_param->get_logger(), "Interrupted while waiting for the service. Exiting.");
    //     return 0;
    //     }
    //     RCLCPP_INFO(node_param->get_logger(), "service not available, waiting again...");
    // }

    // rclcpp::Parameter("serial_port",serial_port,"/dev/ttyUSB0");
    // rclcpp::Parameter("serial_baudrate",serial_baudrate,115200);
    // rclcpp::Parameter("frame_id",frame_id,"laser_radar_Link");
    // rclcpp::Parameter("inverted",inverted,false);
    // rclcpp::Parameter("angle_compensate",angle_compensate,false);
    // rclcpp::Parameter("scan_mode",scan_mode,std::string());
    // rclcpp::Parameter("output_angle_min",output_angle_min,-180);
    // rclcpp::Parameter("output_angle_max",output_angle_max,180);


    std::string scan_mode;
    serial_port="/dev/ttyUSB0";
    frame_id="laser_radar_Link";
    inverted=false;
    angle_compensate=true;
    output_angle_min=-180;
    output_angle_max=180;;

    scan_pub = node_handle->create_publisher<sensor_msgs::msg::LaserScan>("scan",10);
    //RCLCPP_INFO(node_handle->get_logger(),"RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION"");
    u_result     op_result;

    // create the driver instance
    drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);
    if (!drv) {
        //std::cout<<"Create Driver fail, exit" << std::endl;
        RCLCPP_ERROR(node_handle->get_logger(),"Create Driver fail, exit");
        return -2;
    }

    // make connection...
    if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
        RCLCPP_ERROR(node_handle->get_logger(),"Error, cannot bind to the specified serial port %s.",serial_port.c_str());
        //std::cout<<"Error, cannot bind to the specified serial port %s." << std::endl;
        RPlidarDriver::DisposeDriver(drv);
        return -1;
    }

    // get rplidar device info
    if (!getRPLIDARDeviceInfo(drv)) {
        return -1;
    }

    // check health...
    if (!checkRPLIDARHealth(drv)) {
        RPlidarDriver::DisposeDriver(drv);
        return -1;
    }
    drv->startMotor();

    RplidarScanMode 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<RplidarScanMode> allSupportedScanModes;
        op_result = drv->getAllSupportedScanModes(allSupportedScanModes);

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

            if (selectedScanMode == _u16(-1)) {
                RCLCPP_ERROR(node_handle->get_logger(),"scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str());
                for (std::vector<RplidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {
                    RCLCPP_ERROR(node_handle->get_logger(),"\t%s: max_distance: %.1f m, Point number: %.1fK",  iter->scan_mode,
                                iter->max_distance, (1000/iter->us_per_sample));
                    ;
                }
                op_result = RESULT_OPERATION_FAIL;
            } else {
                op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, &current_scan_mode);
            }
        }
    }
    if(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
        angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0);
        if(angle_compensate_multiple < 1)
            angle_compensate_multiple = 1;
        max_distance = current_scan_mode.max_distance;
        RCLCPP_INFO(node_handle->get_logger(),"current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d",  current_scan_mode.scan_mode,
                    current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample), angle_compensate_multiple);
    } else {
        RCLCPP_ERROR(node_handle->get_logger(),"Can not start scan: %08x!", op_result);
    }


    rclcpp::Time start_scan_time;
    rclcpp::Time end_scan_time;
    double scan_duration;

    while(rclcpp::ok()) {
        rplidar_response_measurement_node_hq_t nodes[360*8];
        size_t   count = _countof(nodes);

        start_scan_time = node_handle->now();
        op_result = drv->grabScanDataHq(nodes, count);
        end_scan_time = node_handle->now();
        scan_duration = (end_scan_time - start_scan_time).seconds();
        if (op_result == RESULT_OK) {
            op_result = drv->ascendScanData(nodes, count);

            float angle_min = DEG2RAD(0.0f);
            float angle_max = DEG2RAD(359.0f);


            if (op_result == 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;
                    rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count];
                    memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_hq_t));

                    int 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++) {
                                angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
                            }
                        }
                    }

                    publish_scan(angle_compensate_nodes, angle_compensate_nodes_count,
                                 start_scan_time, scan_duration, inverted,
                                 angle_min, angle_max, max_distance,
                                 frame_id);
                } 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(&nodes[start_node], end_node-start_node +1,
                                 start_scan_time, scan_duration, inverted,
                                 angle_min, angle_max, max_distance,
                                 frame_id);
                }
            } else if (op_result == 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(nodes, count,
                             start_scan_time, scan_duration, inverted,
                             angle_min, angle_max, max_distance,
                             frame_id);
            }
        }

		rclcpp::spin_some(node_handle);
    }
    // done!
    drv->stop();
    drv->stopMotor();
    RPlidarDriver::DisposeDriver(drv);
    return 0;
}

然后在CMakeLists.txt文件下添加编译规则：

set(RPLIDAR_SDK_PATH "./sdk/")

FILE(GLOB RPLIDAR_SDK_SRC 
  "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp"
  "${RPLIDAR_SDK_PATH}/src/hal/*.cpp"
  "${RPLIDAR_SDK_PATH}/src/*.cpp"
)
include_directories(
  ${RPLIDAR_SDK_PATH}/include
  ${RPLIDAR_SDK_PATH}/src
  ${catkin_INCLUDE_DIRS}
)


add_executable(rplidarNode_A1 src/node_A1.cpp ${RPLIDAR_SDK_SRC})
ament_target_dependencies(rplidarNode_A1 rclcpp std_msgs sensor_msgs)

install(TARGETS
rplidarNode_A1
DESTINATION lib/${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

然后回到工作空间下编译

ln@ln-pctogo:~/ros2_ws$ colcon build

一般来说会报以下警告，是其sdk的问题，不耽误使用懒得找解决方法

--- stderr: rplidar_ros                                  
In file included from /home/ln/ros2_ws/src/rplidar_ros/./sdk/include/rplidar.h:39:0,
                 from /home/ln/ros2_ws/src/rplidar_ros/src/node_A1.cpp:7:
/home/ln/ros2_ws/src/rplidar_ros/./sdk/include/rplidar_protocol.h:59:15: warning: ISO C++ forbids zero-size array ‘data’ [-Wpedantic]
     _u8 data[0];
               ^
In file included from /home/ln/ros2_ws/src/rplidar_ros/./sdk/include/rplidar.h:40:0,
                 from /home/ln/ros2_ws/src/rplidar_ros/src/node_A1.cpp:7:
/home/ln/ros2_ws/src/rplidar_ros/./sdk/include/rplidar_cmd.h:244:19: warning: ISO C++ forbids zero-size array ‘payload’ [-Wpedantic]
     _u8  payload[0];
                   ^
/home/ln/ros2_ws/src/rplidar_ros/src/node_A1.cpp: In function ‘int main(int, char**)’:
/home/ln/ros2_ws/src/rplidar_ros/src/node_A1.cpp:337:111: warning: ISO C++ forbids variable length array ‘angle_compensate_nodes’ [-Wvla]
 nse_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count];
                                                                              ^
/home/ln/ros2_ws/src/rplidar_ros/src/node_A1.cpp:341:30: warning: comparison between signed and unsigned integer expressions [-Wsign-compare]
                     for( ; i < count; i++ ) {
                            ~~^~~~~~~
---
Finished <<< rplidar_ros [6.66s]

然后在source一下工作空间下setup.bash文件

ln@ln-pctogo:~/ros2_ws$ source install/setup.bash 

现在就可以运行节点了，当然你先要给设备权限

sudo chmod 777 /dev/ttyUSB0
ros2 run rplidar_ros rplidarNode_A1 

然后打开rviz2

rviz2

将Fixed Frame改为程序中设置好的laser_radar_Link，然后和ros1同样的操作，点击add添加LaserScan可视化插件即可看到点云数据。

下面说一说存在的问题

1.因为还没学习参数获取，所以你只能通过修改程序重新编译的方式设置serial_port、frame_id等参数。
2.因为图省事忽略了stop_motor、stop_motor两个服务，所以你只能通过打开关闭节点来控制激光雷达旋转
3.未用到ros2特性Node Composition,感觉没必要
如果以上三点你都可以忽略那么现在可以使用该节点获取激光雷达数据了
4.当然，测试后代码修改了一点，是关于打印的，感觉应该没有问题，如果不能使用请私信我

git地址（包含其他关于ros2功能包，只需要把rplidar_ros功能包单独拉出来编译即可）：https://github.com/DylanLN/oryxbot_ws-ros2