更新
pcl中rangeImage的视角变换（旋转+平移）看这个
使用ROS和PCL实现pointcloud转rangeimage
主要思路
- pointcloud->rangeimage
- rangeimage->cv::Mat
- cv::Mat->sensor_msgs::Image
下面简单介绍，完整代码见文末
pointcloud to RangeImage
这一块我们使用PCL的rangeImage这个类进行实现。
pt2image_core.h
// 库文件 Pcl2ImgCore类定义
#pragma once
#include <math.h>
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl/range_image/range_image.h>
#include <pcl/range_image/range_image_spherical.h>
#include <opencv2/core/core.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/image_encodings.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/Pose.h>
#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <aiimooc_syz/RangeImageConfig.h> //dynamic reconfigure生成的头文件
// namespace
// {
// typedef aiimooc_syz::RangeImageConfig conf;
// typedef dynamic_reconfigure::Server<conf> RangeImageReconfServer;
// }
class Pcl2ImgCore
{
private:
ros::Subscriber sub_point_cloud_;
ros::Publisher pub_Img_;
// boost::shared_ptr<RangeImageReconfServer> drsv_;
// RangeImage frame
pcl::RangeImage::CoordinateFrame _frame;
// RangeImage resolution
float _ang_res_x;
float _ang_res_y;
// RangeImage angular FoV
float _max_ang_w;
float _max_ang_h;
// Sensor min/max range
float _min_range;
float _max_range;
Eigen::Affine3f camera_pose; // 相机位姿，pcl生成depth_image函数会用到
pcl::PointCloud<pcl::PointXYZ>::Ptr current_pc_ptr; //转换后的点云数据
pcl::RangeImageSpherical::Ptr range_image; // pointcloud生成的range_image
sensor_msgs::ImagePtr msg; // 最后发布的消息格式
cv::Mat _rangeImage; // rangeimage转成图片才能以msg发送出去
std::string encoding ="mono16" ; // 编码格式
public:
Pcl2ImgCore(ros::NodeHandle &nh); //构造函数
~Pcl2ImgCore(); // 析构函数
void point_cb(const sensor_msgs::PointCloud2ConstPtr& in_cloud); // 回调函数
void Spin();
private:
// dynamic reconfigure 的回调函数
void dynamic_callback(aiimooc_syz::RangeImageConfig &config, uint32_t level);
};
pt2image_core.cpp
// Pcl2ImgCore类实现
#include "pt2image_core.h"
// 构造函数
Pcl2ImgCore::Pcl2ImgCore(ros::NodeHandle &nh):
// 初始化成员
_frame(pcl::RangeImage::LASER_FRAME), // 坐标系
_ang_res_x(0.5), //水平角度分辨率
_ang_res_y(0.7),
_max_ang_w(360), //水平角度范围
_max_ang_h(360),
_min_range(0.5),
_max_range(50),
camera_pose(Eigen::Affine3f::Identity()), // 相机位姿，pcl生成depth_image函数会用到
range_image(new pcl::RangeImageSpherical),
current_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>),
msg(new sensor_msgs::Image)
{
std::cout<<"初始化类Pcl2ImgCore"<<std::endl;
// dynamic reconfigure 将这个回调函数放在构造函数内部
dynamic_reconfigure::Server<aiimooc_syz::RangeImageConfig> server;
dynamic_reconfigure::Server<aiimooc_syz::RangeImageConfig>::CallbackType callback;
callback = boost::bind(&Pcl2ImgCore::dynamic_callback, this,_1,_2); // 调用dynamic reconfigure的回调函数
server.setCallback(callback);
sub_point_cloud_=nh.subscribe("/rslidar_points",10,&Pcl2ImgCore::point_cb,this);
pub_Img_=nh.advertise<sensor_msgs::Image>("/range_image", 10); //发布到/image话题
ros::spin();
}
//析构函数
Pcl2ImgCore::~Pcl2ImgCore(){}
void Pcl2ImgCore::Spin(){}
// 回调函数
void Pcl2ImgCore::point_cb(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr){
std::cout<<"--------------start-------------------\nget Pointcloud"<<std::endl;
pcl::fromROSMsg(*in_cloud_ptr, *current_pc_ptr); //ros数据类型转为pcl中的数据类型，下面就使用current_pc_ptr了
// rangeImageSpherial投影
range_image->createFromPointCloud( *current_pc_ptr,
pcl::deg2rad(_ang_res_x),pcl::deg2rad(_ang_res_y),
pcl::deg2rad(_max_ang_w),pcl::deg2rad(_max_ang_h),
camera_pose, // 相机位姿参数，需要指定一个值
pcl::RangeImage::LASER_FRAME,0.0, 0.0f, 0);
std::cout<<*range_image<<std::endl;
// 给range_image设置header
range_image->header.seq = current_pc_ptr->header.seq;
range_image->header.frame_id = current_pc_ptr->header.frame_id;
range_image->header.stamp = current_pc_ptr->header.stamp;
int cols = range_image->width;
int rows = range_image->height;
// sensor_msgs::ImagePtr msg;
// 转换因子
float factor = 1.0f / (_max_range - _min_range);
float offset = -_min_range;
// std::cout<<"factor:\t"<<factor<<std::endl;
// cv::Mat _rangeImage; //最后的OpenCV格式的图像
_rangeImage = cv::Mat::zeros(rows, cols, cv_bridge::getCvType(encoding));
std::cout<<"cols: "<<cols<<","<<"rows: "<<rows<<std::endl;
// 遍历每一个点 生成OpenCV格式的图像
for (int i=0; i<cols; ++i)
{
for (int j=0; j<rows; ++j)
{
float r = range_image->getPoint(i, j).range;
float range = (!std::isinf(r))?
std::max(0.0f, std::min(1.0f, factor * (r + offset))) :
0.0;
_rangeImage.at<ushort>(j, i) = static_cast<ushort>((range) * std::numeric_limits<ushort>::max());
}
}
// 转换为msg
msg=cv_bridge::CvImage(std_msgs::Header(),encoding,_rangeImage).toImageMsg(); // 这里直接使用range_image的header为什么不行？？？
pcl_conversions::fromPCL(range_image->header, msg->header); // header的转变
std::cout<<"in_cloud_ptr->header\n"<<in_cloud_ptr->header<<std::endl;
std::cout<<"current_pc_ptr->header\n"<<current_pc_ptr->header<<std::endl;
std::cout<<"range_image->header\n"<<range_image->header<<std::endl;
std::cout<<"msg->header\n"<<msg->header<<std::endl;
pub_Img_.publish(msg);
std::cout<<"---------------------end----------------------"<<std::endl;
}
// dynamic reconfigure 回调函数
void Pcl2ImgCore::dynamic_callback(aiimooc_syz::RangeImageConfig &config, uint32_t level) {
// 从.cfg文件中获取,传递给成员变量
_ang_res_x = config.ang_res_x;
_ang_res_y = config.ang_res_y;
_max_ang_w = config.max_ang_w;
_max_ang_h = config.max_ang_h;
// 打印
ROS_INFO("Reconfigure Request: %f %f %f %f ",
config.ang_res_x, config.ang_res_y,
config.max_ang_w,
config.max_ang_h
);
}
pt2image_node.cpp
//入口函数
#include "pt2image_core.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "range_image"); // 节点名称
ros::NodeHandle nh;
// if (nh.getParam("/pt_to_image/res_x",_ang_res_x)){
// ROS_INFO("proportional gain set to %f",_ang_res_x);
// }
// else
// {
// ROS_WARN("could not find parameter value /pt_to_image/res_x on parameter server");
// }
// if (nh.getParam("/pt_to_image/res_y",_ang_res_y)){
// ROS_INFO("proportional gain set to %f",_ang_res_y);
// }
// else
// {
// ROS_WARN("could not find parameter value /pt_to_image/res_y on parameter server");
// }
// if (nh.getParam("/pt_to_image/width_range",_max_ang_w)){
// ROS_INFO("proportional gain set to %f",_max_ang_w);
// }
// else
// {
// ROS_WARN("could not find parameter value /pt_to_image/width_range on parameter server");
// }
// if (nh.getParam("/pt_to_image/height_range",_max_ang_h)){
// ROS_INFO("proportional gain set to %f",_max_ang_h);
// }
// else
// {
// ROS_WARN("could not find parameter value /pt_to_image/height_range on parameter server");
// }
// 先将dynamic reconfigure的参数设置到临时变量上
// 创建对象
Pcl2ImgCore core(nh);
return 0;
}
注意：在构造函数中我们把dynamic reconfigure的服务器放在了其中，并将回调函数设置为成员函数，这样就能保证每一次发送参数配置请求都能更新（将dynamic reconfigure放在构造 函数之外是不能动态配置的，只能读取初始的参数值）
ros params
yaml文件
width_range: 180.
height_range: 180.
res_x: 1.0
res_y: 1.0
launch文件
<?xml version="1.0"?>
<launch>
<node name="pt_to_image" type="aiimooc_syz_node" pkg="aiimooc_syz" output="screen" respawn="false">
<rosparam file="$(find aiimooc_syz)/cfg/params.yaml" command="load"/>
</node>
</launch>
node.cpp文件
if (nh.getParam("/pt_to_image/res_x",_ang_res_x)){
ROS_INFO("proportional gain set to %f",_ang_res_x);
}
else
{
ROS_WARN("could not find parameter value /pt_to_image/res_x on parameter server");
}
if (nh.getParam("/pt_to_image/res_y",_ang_res_y)){
ROS_INFO("proportional gain set to %f",_ang_res_y);
}
else
{
ROS_WARN("could not find parameter value /pt_to_image/res_y on parameter server");
}
if (nh.getParam("/pt_to_image/width_range",_max_ang_w)){
ROS_INFO("proportional gain set to %f",_max_ang_w);
}
else
{
ROS_WARN("could not find parameter value /pt_to_image/width_range on parameter server");
}
if (nh.getParam("/pt_to_image/height_range",_max_ang_h)){
ROS_INFO("proportional gain set to %f",_max_ang_h);
}
else
{
ROS_WARN("could not find parameter value /pt_to_image/height_range on parameter server");
}
这样就能读取yaml里面配置的参数了
ros dynamic reconfigure
配置文件：RangeImage.cfg
#!/usr/bin/env python
PACKAGE = "aiimooc_syz"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("ang_res_x", double_t, 0, "resolution in x-direction", 0.5, 0.05, 10)
gen.add("ang_res_y", double_t, 0, "resolution in x-direction", 0.7, 0.05, 10)
gen.add("max_ang_w", double_t, 0, "angle bounds of the sensor in width", 180, 0, 360)
gen.add("max_ang_h", double_t, 0, "angle bounds of the sensor in height", 180, 0, 360)
exit(gen.generate(PACKAGE, "range_image", "RangeImage"))
注意：
这里最后一行的后两个参数是有讲究的。
其中第二个参数’range_image’是要和我们调用这个dynamic reconfigure的节点名称一样！
第三个参数则是和我们这个.cfg文件名称保持一致！
这两点特别重要！
之后再设置权限为可执行文件
chmod a+x RamgeImage.cfg
创建服务端节点
正如上面的core.cpp文件那样，设置一个服务器
dynamic_reconfigure::Server<aiimooc_syz::RangeImageConfig> server;
dynamic_reconfigure::Server<aiimooc_syz::RangeImageConfig>::CallbackType callback;
callback = boost::bind(&Pcl2ImgCore::dynamic_callback, this,_1,_2); // 调用dynamic reconfigure的回调函数
server.setCallback(callback);
回调函数
// dynamic reconfigure 回调函数
void Pcl2ImgCore::dynamic_callback(aiimooc_syz::RangeImageConfig &config, uint32_t level) {
// 从.cfg文件中获取,传递给成员变量
_ang_res_x = config.ang_res_x;
_ang_res_y = config.ang_res_y;
_max_ang_w = config.max_ang_w;
_max_ang_h = config.max_ang_h;
// 打印
ROS_INFO("Reconfigure Request: %f %f %f %f ",
config.ang_res_x, config.ang_res_y,
config.max_ang_w,
config.max_ang_h
);
}
CMakeLists.txt
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/RangeImage.cfg
# cfg/DynReconf2.cfg
)
# 最后别忘记加了这个
add_dependencies(ground_removal_node ${PROJECT_NAME}_gencfg) # dynamic reconfigure 新加的依赖
这一条就像自定义msg一样，会生成一个头文件：(注意一致性)
#include <dynamic_reconfigure/server.h>
#include <aiimooc_syz4/FitPlaneConfig.h> //dynamic reconfigure生成的头文件
动态配置参数GUI
启动该节点后，执行：
rosrun dynamic_tutorials dynamic_reconfigure_node # 已经被下面这条命令替换
rosrun rqt_reconfigure rqt_reconfigure
结果
完整代码
https://github.com/suyunzzz/aiimooc_lesson
参考
pointcloud to rangeimage
ROS 三维激光数据转化为RangeImage
artivis/pointcloud_to_rangeimage
无人驾驶汽车系统入门（二十四）——激光雷达的地面-非地面分割和pcl_ros实践
ros params
ros dynamic reconfigure
使用dynamic_reconfigure实现节点参数动态更新
Setting up Dynamic Reconfigure for a Node(cpp)
ROS探索总结（四十）——dynamic reconfigure
dynamic_reconfigure callback function not called
437
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
