一、引言
1、本博文主要目的是将rslidar_to_velodyne功能包的ros1版本转换为ros2版本
2、内容会包含ROS1到ROS2迁移技巧，是自己总结的一套简单的流程，可以保证ROS2下的代码试跑成功，如果需要将代码进一步转化为类的实现的方式，自己稍作修改就可以了
3、最终会放原始ROS1版本以及修改过后的ROS2版本的代码配置文件和CPP文件供大家对比参考
4、本来是想用现成的开源的ROS2版本的rslidar_to_velodyne，但是没有找到，不知道是因为大家没有需求还是没有开源出来，所以我干脆就自己修改下并开源出来给大家使用，希望能给各位提供帮助（给我一键三连即可(▽)）
5、最后，ros2是真的比ros1方便很多(#.#)
二、整体思路与流程
1、安装虚拟机或者双系统并配置ROS2环境
双系统的话参考我以前的博文，然后ROS2环境大家用小鱼的一键安装即可，网上应该能搜到
2、根据功能包package.xml文件确定所需要的依赖，并根据该依赖创建新的ROS2功能包
3、创建CPP文件并修改对应的CmakeList文件配置
4、修改CPP文件内容，将ROS1的API转化为ROS2
5、配置安装编译LIO-SAM的ROS2版本并用一个速腾雷达bag包进行算法测试
三、ROS1迁移ROS2
3.1 配置ros2安装环境
一键安装，然后你需要同时安装ROS1 Noetic版本和ROS2 Galactic版本，根据自己需要吧，反正ros1和ros2一起安装就可以了，这个之后有用处，也不用担心环境会冲突，这个东西就是配置下环境变量的事儿
wget http://fishros.com/install -O fishros && . fishros
3.2 创建功能包
ROS1版本rslidar_to_velodyne代码文件核心的有三个：CMakeLists.txt、package.xml、rs_to_velodyne.cpp
首先我们看一下ROS1版本package.xml文件：
<?xml version="1.0"?>
<package format="2">
<name>rs_to_velodyne</name>
<version>0.0.0</version>
<description>The rs_to_velodyne package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="hvt@todo.todo">hvt</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/rs_to_velodyne</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_filters</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>message_filters</build_export_depend>
<build_export_depend>pcl_conversions</build_export_depend>
<build_export_depend>pcl_ros</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>message_filters</exec_depend>
<exec_depend>pcl_conversions</exec_depend>
<exec_depend>pcl_ros</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
里面的重点关注一下，可以发现其依赖为message_filters、pcl_conversions、pcl_ros、roscpp、sensor_msgs、std_msgs一共六个需要配置的依赖包记住他们，后面我们创建新的功能包可以用到。
这个时候我们就可以开始创建功能包了
ros2 pkg create rs_to_velodyne --build-type ament_cmake --dependencies rclcpp std_msgs sensor_msgs message_filters pcl_conversions pcl_ros 3.3 修改ROS2版本功能包的package.xml文件
上一步创建功能包其实我们已经把依赖关系添加了，所以这一步并不需要修改了，如果是直接创建功能包的话，需要手动添加自己用到的依赖，这样比较麻烦，所以才先看下ROS1的package.xml文件提前知道需要用到的依赖包这样子给后面节省工作量，这里放一下ROS2版本功能包的package.xml文件
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rs_to_velodyne</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="lixushi@todo.todo">lixushi</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>message_filters</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
3.4 在新的功能包src文件夹下创建rs_to_velodyne.cpp文件，内容就先把ROS1的代码放进去，之后我们再一步一步修改
//#include "utility.h"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
std::string output_type;
static int RING_ID_MAP_RUBY[] = {
3, 66, 33, 96, 11, 74, 41, 104, 19, 82, 49, 112, 27, 90, 57, 120,
35, 98, 1, 64, 43, 106, 9, 72, 51, 114, 17, 80, 59, 122, 25, 88,
67, 34, 97, 0, 75, 42, 105, 8, 83, 50, 113, 16, 91, 58, 121, 24,
99, 2, 65, 32, 107, 10, 73, 40, 115, 18, 81, 48, 123, 26, 89, 56,
7, 70, 37, 100, 15, 78, 45, 108, 23, 86, 53, 116, 31, 94, 61, 124,
39, 102, 5, 68, 47, 110, 13, 76, 55, 118, 21, 84, 63, 126, 29, 92,
71, 38, 101, 4, 79, 46, 109, 12, 87, 54, 117, 20, 95, 62, 125, 28,
103, 6, 69, 36, 111, 14, 77, 44, 119, 22, 85, 52, 127, 30, 93, 60
};
static int RING_ID_MAP_16[] = {
0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8
};
// rslidar和velodyne的格式有微小的区别
// rslidar的点云格式
struct RsPointXYZIRT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring = 0;
double timestamp = 0;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)(double, timestamp, timestamp))
// velodyne的点云格式
struct VelodynePointXYZIRT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)(float, time, time)
)
struct VelodynePointXYZIR {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIR,
(float, x, x)(float, y, y)
(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)
)
ros::Subscriber subRobosensePC;
ros::Publisher pubRobosensePC;
template<typename T>
bool has_nan(T point) {
// remove nan point, or the feature assocaion will crash, the surf point will containing nan points
// pcl remove nan not work normally
// ROS_ERROR("Containing nan point!");
if (pcl_isnan(point.x) || pcl_isnan(point.y) || pcl_isnan(point.z)) {
return true;
} else {
return false;
}
}
template<typename T>
void publish_points(T &new_pc, const sensor_msgs::PointCloud2 &old_msg) {
// pc properties
new_pc->is_dense = true;
// publish
sensor_msgs::PointCloud2 pc_new_msg;
pcl::toROSMsg(*new_pc, pc_new_msg);
pc_new_msg.header = old_msg.header;
pc_new_msg.header.frame_id = "velodyne";
pubRobosensePC.publish(pc_new_msg);
}
void rsHandler_XYZI(sensor_msgs::PointCloud2 pc_msg) {
pcl::PointCloud<pcl::PointXYZI>::Ptr pc(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<VelodynePointXYZIR>::Ptr pc_new(new pcl::PointCloud<VelodynePointXYZIR>());
pcl::fromROSMsg(pc_msg, *pc);
// to new pointcloud
for (int point_id = 0; point_id < pc->points.size(); ++point_id) {
if (has_nan(pc->points[point_id]))
continue;
VelodynePointXYZIR new_point;
new_point.x = pc->points[point_id].x;
new_point.y = pc->points[point_id].y;
new_point.z = pc->points[point_id].z;
new_point.intensity = pc->points[point_id].intensity;
// remap ring id
if (pc->height == 16) {
new_point.ring = RING_ID_MAP_16[point_id / pc->width];
} else if (pc->height == 128) {
new_point.ring = RING_ID_MAP_RUBY[point_id % pc->height];
}
pc_new->points.push_back(new_point);
}
publish_points(pc_new, pc_msg);
}
template<typename T_in_p, typename T_out_p>
void handle_pc_msg(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
T_out_p new_point;
// std::copy(pc->points[point_id].data, pc->points[point_id].data + 4, new_point.data);
new_point.x = pc_in->points[point_id].x;
new_point.y = pc_in->points[point_id].y;
new_point.z = pc_in->points[point_id].z;
new_point.intensity = pc_in->points[point_id].intensity;
// new_point.ring = pc->points[point_id].ring;
// // 计算相对于第一个点的相对时间
// new_point.time = float(pc->points[point_id].timestamp - pc->points[0].timestamp);
pc_out->points.push_back(new_point);
}
}
template<typename T_in_p, typename T_out_p>
void add_ring(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
int valid_point_id = 0;
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
// 跳过nan点
pc_out->points[valid_point_id++].ring = pc_in->points[point_id].ring;
}
}
template<typename T_in_p, typename T_out_p>
void add_time(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
int valid_point_id = 0;
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
// 跳过nan点
pc_out->points[valid_point_id++].time = float(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp);
}
}
void rsHandler_XYZIRT(const sensor_msgs::PointCloud2 &pc_msg) {
pcl::PointCloud<RsPointXYZIRT>::Ptr pc_in(new pcl::PointCloud<RsPointXYZIRT>());
pcl::fromROSMsg(pc_msg, *pc_in);
if (output_type == "XYZIRT") {
pcl::PointCloud<VelodynePointXYZIRT>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIRT>());
handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
add_ring<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
add_time<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
} else if (output_type == "XYZIR") {
pcl::PointCloud<VelodynePointXYZIR>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIR>());
handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
add_ring<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
} else if (output_type == "XYZI") {
pcl::PointCloud<pcl::PointXYZI>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZI>());
handle_pc_msg<RsPointXYZIRT, pcl::PointXYZI>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "rs_converter");
ros::NodeHandle nh;
if (argc < 3) {
ROS_ERROR(
"Please specify input pointcloud type( XYZI or XYZIRT) and output pointcloud type(XYZI, XYZIR, XYZIRT)!!!");
exit(1);
} else {
// 输出点云类型
output_type = argv[2];
if (std::strcmp("XYZI", argv[1]) == 0) {
subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZI);
} else if (std::strcmp("XYZIRT", argv[1]) == 0) {
subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZIRT);
} else {
ROS_ERROR(argv[1]);
ROS_ERROR("Unsupported input pointcloud type. Currently only support XYZI and XYZIRT.");
exit(1);
}
}
pubRobosensePC = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 1);
ROS_INFO("Listening to /rslidar_points ......");
ros::spin();
return 0;
}
3.5 修改新功能包的CMakeLists.txt文件
首先看一下ROS1版本的：
cmake_minimum_required(VERSION 3.0.2)
project(rs_to_velodyne)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
pcl_ros
pcl_conversions
std_msgs
sensor_msgs
)
catkin_package(
INCLUDE_DIRS
DEPENDS PCL
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
link_directories(
include
${PCL_LIBRARY_DIRS}
)
add_executable(rs_to_velodyne src/rs_to_velodyne.cpp)
add_dependencies(rs_to_velodyne ${catkin_EXPORTED_TARGETS})
target_link_libraries(rs_to_velodyne ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
再看一下ROS2版本的：
cmake_minimum_required(VERSION 3.8)
project(rs_to_velodyne)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(message_filters REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
add_executable(rs_to_velodyne src/rs_to_velodyne.cpp)
ament_target_dependencies(
rs_to_velodyne
"rclcpp"
"std_msgs"
"sensor_msgs"
"message_filters"
"pcl_conversions"
"pcl_ros"
)
install(TARGETS
rs_to_velodyne
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
其实只是在新功能包的CMakeLists.txt文件中添加了下面这些内容，如果有其他CPP文件同理创建就可以了
add_executable(rs_to_velodyne src/rs_to_velodyne.cpp)
ament_target_dependencies(
rs_to_velodyne
"rclcpp"
"std_msgs"
"sensor_msgs"
"message_filters"
"pcl_conversions"
"pcl_ros"
)
install(TARGETS
rs_to_velodyne
DESTINATION lib/${PROJECT_NAME})
3.6 最后最关键的就是修改CPP文件了，老规矩先放ROS1版本的，然后我们一步一步去对应修改，会发现其实挺简单的
//#include "utility.h"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
std::string output_type;
static int RING_ID_MAP_RUBY[] = {
3, 66, 33, 96, 11, 74, 41, 104, 19, 82, 49, 112, 27, 90, 57, 120,
35, 98, 1, 64, 43, 106, 9, 72, 51, 114, 17, 80, 59, 122, 25, 88,
67, 34, 97, 0, 75, 42, 105, 8, 83, 50, 113, 16, 91, 58, 121, 24,
99, 2, 65, 32, 107, 10, 73, 40, 115, 18, 81, 48, 123, 26, 89, 56,
7, 70, 37, 100, 15, 78, 45, 108, 23, 86, 53, 116, 31, 94, 61, 124,
39, 102, 5, 68, 47, 110, 13, 76, 55, 118, 21, 84, 63, 126, 29, 92,
71, 38, 101, 4, 79, 46, 109, 12, 87, 54, 117, 20, 95, 62, 125, 28,
103, 6, 69, 36, 111, 14, 77, 44, 119, 22, 85, 52, 127, 30, 93, 60
};
static int RING_ID_MAP_16[] = {
0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8
};
// rslidar和velodyne的格式有微小的区别
// rslidar的点云格式
struct RsPointXYZIRT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring = 0;
double timestamp = 0;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)(double, timestamp, timestamp))
// velodyne的点云格式
struct VelodynePointXYZIRT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)(float, time, time)
)
struct VelodynePointXYZIR {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIR,
(float, x, x)(float, y, y)
(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)
)
ros::Subscriber subRobosensePC;
ros::Publisher pubRobosensePC;
template<typename T>
bool has_nan(T point) {
// remove nan point, or the feature assocaion will crash, the surf point will containing nan points
// pcl remove nan not work normally
// ROS_ERROR("Containing nan point!");
if (pcl_isnan(point.x) || pcl_isnan(point.y) || pcl_isnan(point.z)) {
return true;
} else {
return false;
}
}
template<typename T>
void publish_points(T &new_pc, const sensor_msgs::PointCloud2 &old_msg) {
// pc properties
new_pc->is_dense = true;
// publish
sensor_msgs::PointCloud2 pc_new_msg;
pcl::toROSMsg(*new_pc, pc_new_msg);
pc_new_msg.header = old_msg.header;
pc_new_msg.header.frame_id = "velodyne";
pubRobosensePC.publish(pc_new_msg);
}
void rsHandler_XYZI(sensor_msgs::PointCloud2 pc_msg) {
pcl::PointCloud<pcl::PointXYZI>::Ptr pc(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<VelodynePointXYZIR>::Ptr pc_new(new pcl::PointCloud<VelodynePointXYZIR>());
pcl::fromROSMsg(pc_msg, *pc);
// to new pointcloud
for (int point_id = 0; point_id < pc->points.size(); ++point_id) {
if (has_nan(pc->points[point_id]))
continue;
VelodynePointXYZIR new_point;
new_point.x = pc->points[point_id].x;
new_point.y = pc->points[point_id].y;
new_point.z = pc->points[point_id].z;
new_point.intensity = pc->points[point_id].intensity;
// remap ring id
if (pc->height == 16) {
new_point.ring = RING_ID_MAP_16[point_id / pc->width];
} else if (pc->height == 128) {
new_point.ring = RING_ID_MAP_RUBY[point_id % pc->height];
}
pc_new->points.push_back(new_point);
}
publish_points(pc_new, pc_msg);
}
template<typename T_in_p, typename T_out_p>
void handle_pc_msg(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
T_out_p new_point;
// std::copy(pc->points[point_id].data, pc->points[point_id].data + 4, new_point.data);
new_point.x = pc_in->points[point_id].x;
new_point.y = pc_in->points[point_id].y;
new_point.z = pc_in->points[point_id].z;
new_point.intensity = pc_in->points[point_id].intensity;
// new_point.ring = pc->points[point_id].ring;
// // 计算相对于第一个点的相对时间
// new_point.time = float(pc->points[point_id].timestamp - pc->points[0].timestamp);
pc_out->points.push_back(new_point);
}
}
template<typename T_in_p, typename T_out_p>
void add_ring(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
int valid_point_id = 0;
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
// 跳过nan点
pc_out->points[valid_point_id++].ring = pc_in->points[point_id].ring;
}
}
template<typename T_in_p, typename T_out_p>
void add_time(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
int valid_point_id = 0;
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
// 跳过nan点
pc_out->points[valid_point_id++].time = float(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp);
}
}
void rsHandler_XYZIRT(const sensor_msgs::PointCloud2 &pc_msg) {
pcl::PointCloud<RsPointXYZIRT>::Ptr pc_in(new pcl::PointCloud<RsPointXYZIRT>());
pcl::fromROSMsg(pc_msg, *pc_in);
if (output_type == "XYZIRT") {
pcl::PointCloud<VelodynePointXYZIRT>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIRT>());
handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
add_ring<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
add_time<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
} else if (output_type == "XYZIR") {
pcl::PointCloud<VelodynePointXYZIR>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIR>());
handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
add_ring<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
} else if (output_type == "XYZI") {
pcl::PointCloud<pcl::PointXYZI>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZI>());
handle_pc_msg<RsPointXYZIRT, pcl::PointXYZI>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "rs_converter");
ros::NodeHandle nh;
if (argc < 3) {
ROS_ERROR(
"Please specify input pointcloud type( XYZI or XYZIRT) and output pointcloud type(XYZI, XYZIR, XYZIRT)!!!");
exit(1);
} else {
// 输出点云类型
output_type = argv[2];
if (std::strcmp("XYZI", argv[1]) == 0) {
subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZI);
} else if (std::strcmp("XYZIRT", argv[1]) == 0) {
subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZIRT);
} else {
ROS_ERROR(argv[1]);
ROS_ERROR("Unsupported input pointcloud type. Currently only support XYZI and XYZIRT.");
exit(1);
}
}
pubRobosensePC = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 1);
ROS_INFO("Listening to /rslidar_points ......");
ros::spin();
return 0;
}
第一步修改头文件
将:
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
修改为：
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
第二步修改变量定义
将：
ros::Subscriber subRobosensePC;
ros::Publisher pubRobosensePC;
修改为：
rclcpp::Subscription::SharedPtr subRobosensePC;
rclcpp::Publisher::SharedPtr pubRobosensePC;
第三步修改API
将：
ros::init(argc, argv, “rs_converter”);
ros::NodeHandle nh;
修改为：
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared(“rs_converter”);
将：
ROS_ERROR(“Please specify input pointcloud type( XYZI or XYZIRT) and output pointcloud type(XYZI, XYZIR, XYZIRT)!!!”);
修改为：
RCLCPP_ERROR(node->get_logger(),”Please specify input pointcloud type( XYZI or XYZIRT) and output pointcloud type(XYZI, XYZIR, XYZIRT)!!!”);
当然这里ROS_ERROR不止一个地方哈，都修改掉就行，这里就是举个例子，后文同理
将：
subRobosensePC = nh.subscribe(“/rslidar_points”, 1, rsHandler_XYZI);
修改为：
subRobosensePC = node->create_subscription(“/rslidar_points”, 1, rsHandler_XYZI);
将：
pubRobosensePC = nh.advertise(“/velodyne_points”, 1);
修改为：
pubRobosensePC = node->create_publisher(“/velodyne_points”, 1);
将：
ROS_INFO(“Listening to /rslidar_points ……”);
ros::spin();
修改为：
RCLCPP_INFO(node->get_logger(), “Listening to /rslidar_points ……”);
rclcpp::spin(node);
然后因为这里的订阅者和发布者都是指针类型，所以其成员函数的调用得用”->“的方式替换到”.”的方式，所以要将：
pubRobosensePC.publish(pc_new_msg);
修改为：
pubRobosensePC->publish(pc_new_msg);
以上就是需要修改的全部内容了，如果是其他开源代码需要做移植也是同理，但是呢ROS2其实要把发布者订阅者用继承node的方式进行调用，需要打包成一个类，所以后续还是需要优化一下的，但是我这样修改的代码也是可以使用的（只是我暂时没空改┭┮﹏┭┮）
最后放一下ROS2版本完整的CPP代码叭：
//#include "utility.h"
#include <rclcpp/rclcpp.hpp>
#include <cstring>
#include <cstdlib>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
std::string output_type;
static int RING_ID_MAP_RUBY[] = {
3, 66, 33, 96, 11, 74, 41, 104, 19, 82, 49, 112, 27, 90, 57, 120,
35, 98, 1, 64, 43, 106, 9, 72, 51, 114, 17, 80, 59, 122, 25, 88,
67, 34, 97, 0, 75, 42, 105, 8, 83, 50, 113, 16, 91, 58, 121, 24,
99, 2, 65, 32, 107, 10, 73, 40, 115, 18, 81, 48, 123, 26, 89, 56,
7, 70, 37, 100, 15, 78, 45, 108, 23, 86, 53, 116, 31, 94, 61, 124,
39, 102, 5, 68, 47, 110, 13, 76, 55, 118, 21, 84, 63, 126, 29, 92,
71, 38, 101, 4, 79, 46, 109, 12, 87, 54, 117, 20, 95, 62, 125, 28,
103, 6, 69, 36, 111, 14, 77, 44, 119, 22, 85, 52, 127, 30, 93, 60
};
static int RING_ID_MAP_16[] = {
0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8
};
// rslidar和velodyne的格式有微小的区别
// rslidar的点云格式
struct RsPointXYZIRT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring = 0;
double timestamp = 0;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)(double, timestamp, timestamp))
// velodyne的点云格式
struct VelodynePointXYZIRT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)(float, time, time)
)
struct VelodynePointXYZIR {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIR,
(float, x, x)(float, y, y)
(float, z, z)(float, intensity, intensity)
(uint16_t, ring, ring)
)
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subRobosensePC;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubRobosensePC;
template<typename T>
bool has_nan(T point) {
// remove nan point, or the feature assocaion will crash, the surf point will containing nan points
// pcl remove nan not work normally
// ROS_ERROR("Containing nan point!");
if (pcl_isnan(point.x) || pcl_isnan(point.y) || pcl_isnan(point.z)) {
return true;
} else {
return false;
}
}
template<typename T>
void publish_points(T &new_pc, const sensor_msgs::msg::PointCloud2 &old_msg) {
// pc properties
new_pc->is_dense = true;
// publish
sensor_msgs::msg::PointCloud2 pc_new_msg;
pcl::toROSMsg(*new_pc, pc_new_msg);
pc_new_msg.header = old_msg.header;
pc_new_msg.header.frame_id = "velodyne";
pubRobosensePC->publish(pc_new_msg);
}
void rsHandler_XYZI(sensor_msgs::msg::PointCloud2 pc_msg) {
pcl::PointCloud<pcl::PointXYZI>::Ptr pc(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<VelodynePointXYZIR>::Ptr pc_new(new pcl::PointCloud<VelodynePointXYZIR>());
pcl::fromROSMsg(pc_msg, *pc);
// to new pointcloud
for (int point_id = 0; point_id < pc->points.size(); ++point_id) {
if (has_nan(pc->points[point_id]))
continue;
VelodynePointXYZIR new_point;
new_point.x = pc->points[point_id].x;
new_point.y = pc->points[point_id].y;
new_point.z = pc->points[point_id].z;
new_point.intensity = pc->points[point_id].intensity;
// remap ring id
if (pc->height == 16) {
new_point.ring = RING_ID_MAP_16[point_id / pc->width];
} else if (pc->height == 128) {
new_point.ring = RING_ID_MAP_RUBY[point_id % pc->height];
}
pc_new->points.push_back(new_point);
}
publish_points(pc_new, pc_msg);
}
template<typename T_in_p, typename T_out_p>
void handle_pc_msg(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
T_out_p new_point;
// std::copy(pc->points[point_id].data, pc->points[point_id].data + 4, new_point.data);
new_point.x = pc_in->points[point_id].x;
new_point.y = pc_in->points[point_id].y;
new_point.z = pc_in->points[point_id].z;
new_point.intensity = pc_in->points[point_id].intensity;
// new_point.ring = pc->points[point_id].ring;
// // 计算相对于第一个点的相对时间
// new_point.time = float(pc->points[point_id].timestamp - pc->points[0].timestamp);
pc_out->points.push_back(new_point);
}
}
template<typename T_in_p, typename T_out_p>
void add_ring(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
int valid_point_id = 0;
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
// 跳过nan点
pc_out->points[valid_point_id++].ring = pc_in->points[point_id].ring;
}
}
template<typename T_in_p, typename T_out_p>
void add_time(const typename pcl::PointCloud<T_in_p>::Ptr &pc_in,
const typename pcl::PointCloud<T_out_p>::Ptr &pc_out) {
// to new pointcloud
int valid_point_id = 0;
for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
if (has_nan(pc_in->points[point_id]))
continue;
// 跳过nan点
pc_out->points[valid_point_id++].time = float(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp);
}
}
void rsHandler_XYZIRT(const sensor_msgs::msg::PointCloud2 &pc_msg) {
pcl::PointCloud<RsPointXYZIRT>::Ptr pc_in(new pcl::PointCloud<RsPointXYZIRT>());
pcl::fromROSMsg(pc_msg, *pc_in);
if (output_type == "XYZIRT") {
pcl::PointCloud<VelodynePointXYZIRT>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIRT>());
handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
add_ring<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
add_time<RsPointXYZIRT, VelodynePointXYZIRT>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
} else if (output_type == "XYZIR") {
pcl::PointCloud<VelodynePointXYZIR>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIR>());
handle_pc_msg<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
add_ring<RsPointXYZIRT, VelodynePointXYZIR>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
} else if (output_type == "XYZI") {
pcl::PointCloud<pcl::PointXYZI>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZI>());
handle_pc_msg<RsPointXYZIRT, pcl::PointXYZI>(pc_in, pc_out);
publish_points(pc_out, pc_msg);
}
}
int main(int argc, char **argv) {
//ros::init(argc, argv, "rs_converter");
//ros::NodeHandle nh;
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("rs_converter");
if (argc < 3) {
RCLCPP_ERROR(node->get_logger(),
"Please specify input pointcloud type( XYZI or XYZIRT) and output pointcloud type(XYZI, XYZIR, XYZIRT)!!!");
exit(1);
} else {
// 输出点云类型
output_type = argv[2];
if (std::strcmp("XYZI", argv[1]) == 0) {
//subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZI);
subRobosensePC = node->create_subscription<sensor_msgs::msg::PointCloud2>("/rslidar_points", 1, rsHandler_XYZI);
} else if (std::strcmp("XYZIRT", argv[1]) == 0) {
//subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZIRT);
subRobosensePC = node->create_subscription<sensor_msgs::msg::PointCloud2>("/rslidar_points", 1, rsHandler_XYZIRT);
} else {
RCLCPP_ERROR(node->get_logger(), argv[1]);
RCLCPP_ERROR(node->get_logger(), "Unsupported input pointcloud type. Currently only support XYZI and XYZIRT.");
exit(1);
}
}
//pubRobosensePC = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 1);
pubRobosensePC = node->create_publisher<sensor_msgs::msg::PointCloud2>("/velodyne_points", 1);
RCLCPP_INFO(node->get_logger(), "Listening to /rslidar_points ......");
rclcpp::spin(node);
return 0;
}
大家只需要将我的博客内容的：CMakeLists.txt、package.xml、rs_to_velodyne.cpp文件内容拷贝到到自己的文件就可以直接使用了
四、ROS2版本LIO-SAM试跑
4.1 抓包获取速腾雷达的IP地址
这一小节可以直接跳过，我自己记录用的，因为我的雷达被别人使用过，但是我又不知道IP地址是设置的啥，所以记录一下用抓包的方式获取IP
wireshark进行抓包测试
#wireshark进行抓包测试
sudo apt-get install wireshark
sudo wireshark
可以获取我的雷达ip 为192.168.1.123
主机地址 192.168.1.101（电脑ip需要设置成这个）
ping 192.168.1.123 可以检测是否正常通信
可视化一下看看是否能正常出点云图：
#ros+rviz可视化
roslaunch rslidar_sdk start.launch 
4.2 数据集准备
可能大家手上只有ROS1的bag包，这样子ros2下的算法肯定不能直接用，这个时候就需要将ROS1的bag包转换到ROS2下面去
第一步要确定自己同时安装了ROS1和ROS2
第二步配置环境变量，添加：
source /opt/ros/galactic/setup.bash
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/noetic/lib
第三步安装依赖并运行bag包并订阅查看效果：
sudo apt install -y libroscpp-serialization0d ros-galactic-nmea-msgs ros-noetic-nmea-msgs
sudo apt install -y ros-galactic-rosbag2-bag-v2-plugins ros-galactic-rosbag2-storage ros-galactic-rosbag2-storage-default-plugins ros-galactic-ros2bag ros-galactic-rosbag2-transport
ros2 bag info -s rosbag_v2 hdl_400.bag #查看bag包信息
ros2 bag play -s rosbag_v2 hdl_400.bag #这一步就可以直接将ROS1话题转ROS2了4.3 配置LIO-SAM
sudo apt install ros-galactic-perception-pcl \
ros-galactic-pcl-msgs \
ros-galactic-vision-opencv \
ros-galactic-xacro
sudo add-apt-repository ppa:borglab/gtsam-release-4.0
sudo apt install libgtsam-dev libgtsam-unstable-dev
mkdir -p ~/ros2_slam_3d_ws/src
cd ~/ros2_slam_3d_ws/src
git clone -b ros2 https://github.com/TixiaoShan/LIO-SAM.git
colcon build
这里我遇到了一个报错，额就是我把.hpp文件改成.h就可以了，具体报错内容没有记录下来
4.4 运行LIO-SAM
这一步包含启动lio-sam，启动雷达数据转换节点以及bag包播放、保存地图的操作
ros2 launch lio_sam run.launch.py
ros2 run rs_to_velodyne rs_to_velodyne XYZIRT XYZIRT
ros2 bag play -s rosbag_v2 33.bag
ros2 service call /lio_sam/save_map lio_sam/srv/SaveMap "{resolution: 0.2, destination: /Downloads/LOAM2}"4.5 效果展示
这是我自己在园区采集的数据集，有问题随时私信我~~看到的话会回的
五、参考文献
[1]https://blog.csdn.net/qq_16539009/article/details/124060944
[2]https://www.ncnynl.com/archives/202210/5482.html
[3]https://www.ncnynl.com/archives/202209/5469.html
[4]https://blog.csdn.net/kinderkindme/article/details/127862204
[5]https://www.guyuehome.com/36725
[6]传感器标定参考：https://blog.csdn.net/weixin_42141088/article/details/118000544
[7]算法源码注释：https://blog.csdn.net/zkk9527/article/details/117957067
评论（0）
您还未登录，请登录后发表或查看评论