思岚激光雷达rplidar从ROS 1到ROS 2的移植
目录
前言
由于需要将ROS 1下项目升级到ROS 2，用到了思岚的激光雷达，但目前似乎思岚官方还没有在ROS 2上的package，因此做了一个移植。移植过程参考了ROS 2官方的说明，尽最大可能按照ROS 2新架构的规范做移植，希望本文能对同样在ROS 2上做开发的同仁起到帮助
1. 创建rplida_ros的package
在终端输入命令：
cd ~/dev_ws/src
进入workspace的src目录，继续在终端输入命令：
ros2 pkg create --build-type ament_cmake rplidar_ros2 --dependencies rclcpp sensor_msgs std_srvs
package创建后打开其“package.xml”，参照ROS中rplidar的package.xml文件修改必要的信息，比如版本号，能够帮助查看是否需要后续的更新，如下图：
2. 拷贝思岚rplidar的sdk以及rivz包
拷贝到新建package rplidar_ros2的根目录下，如下图：
进入sdk目录，复制“node.cpp”为“node_ros2.cpp”作为ROS 2下的源码文件，同时保留了ROS下的“node.cpp”作为备份，如下图：
3. 为CMakeLists添加rplidar sdk和node的目标编译文件，以及ROS 2 run命令在寻找node时的路径
打开package的“CMakeLists.txt”，在“find_package”前添加如下语句：
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"
)
即指定rplidar sdk路径的变量“RPLIDAR_SDK_PATH”，同时将rplidar sdk所需要的文件glob到变量“RPLIDAR_SDK_SRC”，便于后续添加目标。关于cmake的glob的说明参考官网链接：
https://cmake.org/cmake/help/v3.0/command/file.html
在“find_package”后添加如下语句：
add_executable(rplidarNode src/node_ros2.cpp ${RPLIDAR_SDK_SRC})
ament_target_dependencies(rplidarNode rclcpp sensor_msgs std_srvs)
install(TARGETS
rplidarNode
DESTINATION lib/${PROJECT_NAME})
即为“rplidarNode”添加编译目标文件“node_ros2.cpp”、sdk文件、以及ROS 2 run命令查找node时的路径，如下图：
4. 检查依赖问题
在终端输入命令：
rosdep install -i --from-path src --rosdistro foxy -y
如果依赖都已正确设置或安装，则如下图提示：
否则，在终端运行命令：
rosdep update
更新并安装必要的依赖
5. 尝试编译，以解决CMakeLists问题
在终端输入命令：
colcon build --packages-select rplidar_ros2
编译首先提示找不到“ros/ros.h”，如下图：
因为在ROS 2架构下变为了“rclcpp/rclcpp.hpp“，同理，后面的两个#include，即“sensor_msgs/LaserScan.h”和“std_srvs/Empty.h”，也分别变为了“<sensor_msgs/msg/laser_scan.hpp>”和“<std_srvs/srv/empty.hpp>”，也都需要替换，如下图：
在终端再次输入编译命令：
colcon build --packages-select rplidar_ros2
此时编译提示找不到“rplidar.h”，如下图：
该文件属于rplidar的头文件，在sdk/include目录下，这个问题表明CMakeLists缺少了include directories的声明，需要添加。打开package的“CMakeLists.txt”，在“install”后添加如下语句：
target_include_directories(rplidarNode
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/sdk/include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/sdk/src>
$<INSTALL_INTERFACE:include>)
即加入rplidar sdk的include和src两个路径，如下图：
NOTE：关于ROS 2的ament_cmake说明可以参看官网链接：
https://index.ros.org/doc/ros2/Tutorials/Ament-CMake-Documentation/
6. 适应ROS 2架构的代码移植
ROS 2相比ROS在构建架构上做出了很大的变化，即引入了DDS（Data Distribution Service）来优化ROS中通信，关于DDS的引入的考虑可以参考官网链接：
https://design.ros2.org/articles/ros_on_dds.html
由于DDS的引入，代码由ROS到ROS 2最大的变化在于node的引用方式上，本文对rplidar代码的移植参考了ROS 2说明中的三个示例，Writing a simple publisher and subscriber (C++)、Writing a simple service and client (C++)、Using parameters in a class (C++)，它们的官网链接分别为：
https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/
https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Service-And-Client/
https://index.ros.org/doc/ros2/Tutorials/Using-Parameters-In-A-Class-CPP/
首先，需要从rclcpp::Node继承我们自己的node，本文中为“rplidarROS2”。这里重点介绍继承类rplidarROS2的构造函数，因为该函数的实现过程几乎涵盖了ROS到ROS 2整个代码移植的关键变化，以及由此带来node的main函数的变化
首先看继承类rplidarROS2及其构造函数：
class rplidarROS2 : public rclcpp::Node
{
public:
rplidarROS2()
: Node("rplidar_ros2")
, driver_(nullptr)
, clock_(RCL_ROS_TIME)
, angle_compensate_multiple_(1) // it stand of angle compensate at per 1 degree
, frequency_(5.5)
, channel_type_("serial")
, tcp_ip_("192.168.0.7")
, tcp_port_(20108)
, serial_port_("/dev/ttyUSB0")
, serial_baudrate_(115200/*256000*/) // ros run for A1 A2, change to 256000 if A3;
, frame_id_("laser_frame")
, inverted_(false)
, angle_compensate_(false)
, scan_mode_(std::string())
, max_distance_(8.0)
, screened_begin_(91)
, screened_end_(179)
{
RCLCPP_INFO(get_logger(), "RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION"");
declare_parameters(); // !!!NOTE
get_parameters();
rclcpp::QoS qos(rclcpp::KeepLast(50)); // !!!NOTE
scan_publisher_ = create_publisher<sensor_msgs::msg::LaserScan>("scan", qos); // !!!NOTE
start_motor_server_ = create_service<std_srvs::srv::Empty>("start_motor", std::bind(&rplidarROS2::start_motor, this, std::placeholders::_1, std::placeholders::_2)); // !!!NOTE
stop_motor_server_ = create_service<std_srvs::srv::Empty>("stop_motor", std::bind(&rplidarROS2::stop_motor, this, std::placeholders::_1, std::placeholders::_2));
timer_ = create_wall_timer(std::chrono::milliseconds(int(ceil(1000.0 / frequency_))), std::bind(&rplidarROS2::spin, this)); // !!!NOTE
connect_driver();
check_scan_mode();
};
// other codes but screened for space reason
}
该代码片段即为继承于rclcpp::Node的rplidarROS2类及其构造函数，其他部分未给出，将在后面的源代码中一并给出。本文出于尽最大可能保证所移植代码与rplidar在ROS下的代码一致或相似的目的，将rplidarROS2类作为inline的方式写入了同一个文件“node_ROS2.cpp”，其也可以写入独立的.h和.cpp文件
该代码片段中带有“ // !!!NOTE”注释的部分是需要着重关注的变化点：
a. declare_parameters()
该函数的源代码如下：
void declare_parameters()
{
declare_parameter<double>("frequency", frequency_);
declare_parameter<std::string>("channel_type", channel_type_);
declare_parameter<std::string>("tcp_ip", tcp_ip_);
declare_parameter<int>("tcp_port", tcp_port_);
declare_parameter<std::string>("serial_port", serial_port_);
declare_parameter<int>("serial_baudrate", serial_baudrate_);
declare_parameter<std::string>("frame_id", frame_id_);
declare_parameter<bool>("inverted", inverted_);
declare_parameter<bool>("angle_compensate", angle_compensate_);
declare_parameter<std::string>("scan_mode", scan_mode_);
declare_parameter<double>("max_distance", max_distance_);
declare_parameter<int>("screened_begin", screened_begin_);
declare_parameter<int>("screened_end", screened_end_);
}
ROS 2从版本Dashing，就要求在客户端读取或者设置参数时首先需要声明参数，该变化的好处是客户端代码能够通过自身对参数的声明以此判断ParameterDescriptor中定义的参数是否合法。因为在ROS 2中已经没有global parameter的概念，所有变量都基于其node，因此有对node变量的ParameterDescriptor。同时，ROS 2也兼容了不声明参数的方式，通过在node构造函数中设置变量“allow_undeclared_parameters”以及“automatically_declare_parameters_from_overrides”为“true”来实现直接读取或设置参数的功能，可以参考下面的示例程序链接：
https://github.com/ros2/demos/blob/75cf923f9f3a33be456db1785f91e503c4bee16d/demo_nodes_cpp/src/parameters/parameter_blackboard.cpp#L29
关于声明参数的说明可以参考官网链接：
https://index.ros.org/doc/ros2/Releases/Release-Dashing-Diademata/#declaring-parameters
b. rclcpp::QoS qos(rclcpp::KeepLast(50))和scan_publisher_ = create_publisher…
该代码是因为引入DDS后的变化之一。ROS的通信基于TCP，引入了DDS后，ROS 2的通信就基于了UDP，由于UDP不像TCP是可靠连接，因此DDS通过QoS（Quality of Service）配置机制，依据不同的配置能够在UDP基础上实现与TCP类似的可靠连接
回想在ROS中发布消息的过程，以ROS rplidar为例：
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
该代码两个参数topic和队列缓存数量1000，在ROS 2的QoS中可以通过组合配置“History”和“Depth”两个原则组合实现，即代码中的rclcpp::KeepLast(50)。详细的QoS配置及其说明，参考官网链接：
https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings/
NOTE：ROS rplidar发布消息队列缓存数量设置为了1000，不知道是出于什么原因，按经验来看，50就足够了
c. start_motor_server_ = create_service…和stop_motor_server_ = create_service…
由于使用了类及类成员函数的方式，start motor server和stop motor server两个node service的callback函数使用了bind以及用于表明callback形参数量的placeholders。也可以将callback函数定义为static静态类型，直接输入callback函数的地址。本文中由于callback需要访问类成员变量，static函数会引入额外的重构，因此采用了bind方式
d. timer_ = create_wall_timer…
函数create_wall_timer依据形参period的时间，周期性的调用给定的callback函数spin，本文考虑到rplidar激光扫描频率的可变性并能适配不同频率的rplidar产品，将period设置为参数变量，即可以通过launch方式设置
回想ROS rplidar中main函数while循环处理扫描的过程，其调用了rplidar驱动中的函数“grabScanDataHq”，该函数是一个同步函数，每一次扫描处理的时间决定于rplidar的扫描频率，比如5.5Hz即为182ms左右，因此wall timer调用spin函数的周期，设置为基于launch输入参数的频率计算得到的时间周期
本文在wall timer中调用了spin函数，该函数即是ROS rplidar中main函数的while循环部分。因为spin本文也设置为类成员函数，因此同样使用了bind方式的callback调用
最后，在引入了node继承类后，main函数会变得非常简洁，下面为main函数代码：
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv); // !!!NOTE
try
{
auto node = std::make_shared<rplidarROS2>(); // !!!NOTE
rclcpp::spin(node); // !!!NOTE
rclcpp::shutdown();
}
catch (const std::runtime_error& error)
{
printf((std::string(error.what()) + "\n").c_str());
return -1;
}
return 0;
}
main函数和ROS一致的地方就是进行初始化，区别在ROS 2中变为了rclcpp::init(argc, argv)。同时因为继承类rplidarROS2定义了所有关于激光雷达的操作，main函数只需要创建rplidarROS2的node，即auto node = std::make_shared()，并开启node的spin即可，即rclcpp::spin(node)。整个main函数变得非常简洁直观，这也是ROS 2架构带来的一大变化
7. 再次编译移植后代码
保存移植后代码即node_ROS2.cpp，在终端再次输入命令：
colcon build --packages-select rplidar_ros2
一切顺利的化，终端将输出编译完成的信息，如下图：
在终端继续输入命令：
. install/setup.sh
将package加入setup bash
NOTE：由于本文基于最大可能保持与ROS rplidar的一致性，便于后续如果思岚官方有更新时能够快速merge，因此没有处理编译warning
在终端继续输入命令：
ros2 run rplidar_ros2 rplidarNode
激光雷达node将运行，如下图：
NOTE：第一次运行时可能会出现提示“Error, cannot bind to the specified serial port /dev/ttyUSB0.”，可以通过下面命令：
sudo gpasswd --add ${USER} dialout
重启后解决
激光雷达运行后，可以通过topic echo命令查看/scan消息，检验移植的正确性。打开一个新终端输入命令：
ros2 topic echo /scan
将输入当前激光雷达发布的消息/scan，如下图：
TIP：通过命令ros2 topic list查看所有运行node发布的消息，如下图：
8. launch
代码移植后可以通过命令ros2 run命令运行，但通常在ROS下需要组合多个node运行，比如navigation，因此还需要launch方式实现。ROS 2引入了python代码的launch方式，详细内容以及设计思想参考官网链接：
https://index.ros.org/doc/ros2/Tutorials/Launch-system/
https://design.ros2.org/articles/roslaunch.html
但同时，ROS 2也兼容ROS下xml的launch文件，但做了部分修改，参考官网链接：
https://index.ros.org/doc/ros2/Tutorials/Launch-files-migration-guide/
本文采用第二中方式，即基于ROS下已有的xml launch文件依据ROS 2做适配性修改
a. 为package rplidar_ros2建立launch文件夹
进入rplidar_ros2的根目录，新建文件夹lauch，如下图：
b. 拷贝ROS rplidar的launch文件
在rplidar中特定产品的运行都有两个对应的launch文件，比如产品A1的launch文件为“rplidar.launch”和“view_rplidar.launch”，本文将以这两个文件做说明，其他产品的处理方式是一致的
拷贝ROS rplidar\launch目录下的文件“rplidar.launch”和“view_rplidar.launch”到ROS 2的rplidar_ros2\launch目录下，如下图：
c. rplidar.launch的ROS 2 launch适配修改
打开rplidar.launch修改为下面语句：
<launch>
<node name="rplidarNode" pkg="rplidar_ros2" exec="rplidarNode" output="screen">
<param name="frequency" value="5.5"/>
<param name="serial_port" value="/dev/lidar"/>
<param name="serial_baudrate" value="115200"/><!--A1/A2 -->
<!--param name="serial_baudrate" value="256000"--><!--A3 -->
<param name="frame_id" value="laser"/>
<param name="inverted" value="false"/>
<param name="angle_compensate" value="true"/>
<param name="scan_mode" value="Standard"/>
<param name="screened_begin" value="101"/>
<param name="screened_end" value="259"/>
<param name="max_distance" value="8.0"/>
</node>
</launch>
ROS 2中node没有了type属性，变为了“exe”属性，param没有了type属性，变为依据用户填入数据自动判断，详细区别参考官网链接：
https://index.ros.org/doc/ros2/Tutorials/Launch-files-migration-guide/
保存修改后在终端输入命令：
ros2 launch src/rplidar_ros2/launch/rplidar.launch
激光雷达node将运行，如下图：
NOTE：仔细观察run命令和launch命令的两种方式会发现“Point number”的数量分别为4.0K和2.0K。这是由于launch运行使用了我们指定的参数而得到不同的激光雷达配置结果，具体区别是“angle_compensate”以及“scan_mode”，可以回想在构造函数中上述两个参数与rplidar.launch中默认值及设定值的区别
d. 添加package的xml launch文件
仔细回想ROS中launch的调用方式：roslaunch package_name target.launch，对比上述运行步骤中命令ros2 launch path/target.launch，会发现ROS 2中运行launch不再需要指定package，直接给出目标launch的路径即可，这是ROS 2 launch system带来的另一个变化。有时候也许launch文件的路径不容易记忆，通过package和TAB键能帮助联想出目标launch文件的方式依然具有优势，ROS 2兼容了该方式，可以通过CMakeLists文件中实现
打开rplidar_ROS2根目录下的“CMakeLists.txt”文件，在文件最后但“ament_package()”之前，增加如下语句：
# Install launch files.
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}/
)
在终端输入命令：
cd ~/dev_ws
colcon build --packages-select rplidar_ros2
重新编译rplidar_ros2，继续在终端输入命令：
. install/setup.sh
将package加入setup bash。此时通过终端输入package和其target.launch的命令：
ros2 launch rplidar_ros2 rplidar.launch
即可通过tab联想目标launch，如下图：
e. view_rplidar.launch以及rplidar.rviz的ROS 2 launch适配修改
打开view_rplidar.launch修改为下面语句：
<!--
Used for visualising rplidar in action.
It requires rplidar.launch.
-->
<launch>
<group>
<!--<push-ros-namespace namespace="rplidar_ros2"/> with user defined ns-->
<include file="/home/whi/dev_ws/src/rplidar_ros2/launch/rplidar.launch"/>
</group>
<node name="rviz2" pkg="rviz2" exec="rviz2" args="-d /home/whi/dev_ws/src/rplidar_ros2/rviz/rplidar.rviz"/>
</launch>
ROS 2中include标签必须被group标签包含，同时动态的$(find rplidar_ros)被取消，因此这里输入了绝对路径
NOTE：在ROS 2 Design中关于动态配置有介绍built-in的$(find-pkg-prefix )但本文尝试未能成功
另外需要修改的是rviz目录下的rplidar.rviz，ROS 2中rviz也升级到了rviz 2，大部分configuration也发生了相应的变化，因此本文不在之前ROS下的rviz上做修改，而是通过手动过程在rviz中添加相应的frame和plugin，并另存rviz2下的configuration文件
在终端运行刚才修改的view_rplidar.launch，输入命令：
ros2 launch src/rplidar_ros2/launch/view_rplidar.launch
运行后可以看到终端输出激光雷达成功运行，并打开了rviz，但rviz为默认初始配置，不能显示激光雷达点云
首先将“Fixed Frame”由map改为laser；再添加“LaserScan”插件，并将“topic”设置为/scan，此时rviz的显示区域将能显示激光雷达的点云，如下图：
此时的点云不明显，把LaserScan插件的三个属性“Size”，“ColorTransform”，“Axis”分别设置为“0.03”，“AxisColor”，“Z”，此时点云将变得更明显，如下图：
点击菜单“File->Save Config As”，存储该配置到rviz目录，命名为rplidar.rviz，如下图：
关闭终端中运行的launch，再次运行view_rplidar.launch，在终端输入命令：
ros2 launch src/rplidar_ros2/launch/view_rplidar.launch
此时将看到激光雷达成功运行，以及我们需要的rviz显示，如下图：
总结
至此，思岚激光雷达从ROS到ROS 2的移植工作已经完成，为了支持思岚激光雷达的其他系列产品，可以依据上述步骤继续修改其他launch文件
本文的移植代码已经从源代码端考虑了不同产品频率的兼容性，即通过设置变量“frequency”实现不同产品频率的配置，在移植其他产品launch文件时需要关注并检查该参数变量是否符合产品特性设置
同时，本文的移植代码加入了扫描角度的屏蔽范围设置，即参数“screened_begin”和“screened_end”
本文源代码的github地址：
https://github.com/xinjuezou-whi/rplidar_ros2