2d雷达的原始数据生成点云地图pcd
sensor_msgs::LaserScan转换为sensor_msgs::PointCloud2
主要用到了transformLaserScanToPointCloud()这个函数,
订阅消息结构为sensor_msgs::LaserScan的话题scan,
发布消息结构为sensor_msgs::PointCloud2的话题cloud2
完整的功能包上传至
https://download.csdn.net/download/zhaohaowu/21227408
或者
1、创建功能包
catkin_create_pkg LaserScan2PointCloud2 roscpp rospy tf sensor_msgs laser_geometry
2、CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(LaserScan2PointCloud2)
find_package(catkin REQUIRED COMPONENTS
laser_geometry
roscpp
rospy
sensor_msgs
tf
)
catkin_package(
CATKIN_DEPENDS laser_geometry roscpp rospy sensor_msgs tf
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(LaserScan2PointCloud2 src/LaserScan2PointCloud2.cpp)
target_link_libraries(LaserScan2PointCloud2
${catkin_LIBRARIES}
)
3、src写两个文件
LaserScan2PointCloud2.cpp
#include "LaserScan2PointCloud2.h"
LaserScan2PointCloud2::LaserScan2PointCloud2(){
sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 100, &LaserScan2PointCloud2::callback, this);
pub = n.advertise<sensor_msgs::PointCloud2>("/cloud2", 100);
listener.setExtrapolationLimit(ros::Duration(0.1));
}
void LaserScan2PointCloud2::callback(const sensor_msgs::LaserScan::ConstPtr& scan){
sensor_msgs::PointCloud2 cloud2;
transformer.transformLaserScanToPointCloud("laser", *scan, cloud2, listener);
pub.publish(cloud2);
}
int main(int argc, char** argv){
ros::init(argc, argv, "LaserScan2PointCloud2");
LaserScan2PointCloud2 LaserScan2PointCloud2;
ros::spin();
return 0;
}
LaserScan2PointCloud2.h
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
class LaserScan2PointCloud2{
private:
ros::NodeHandle n;
laser_geometry::LaserProjection transformer;
tf::TransformListener listener;
ros::Publisher pub;
ros::Subscriber sub;
public:
LaserScan2PointCloud2();
void callback(const sensor_msgs::LaserScan::ConstPtr& scan);
};
4、消息结构为sensor_msgs::PointCloud2的话题cloud2(a.bag)转换一下话题名字points_raw(b.bag)
rosparam set use_sim_time true
rosbag record /points_raw
rosbag play --clock a.bag /cloud2:=/points_raw
这个步骤可以不要,在上面代码里将cloud2的话题直接改为points_raw
5、安装autoware
https://blog.csdn.net/zhaohaowu/article/details/119839147
6、消息结构为sensor_msgs::PointCloud2的话题points_raw以b.bag格式转换为pcd(a.pcd)
roslaunch runtime_manager runtime_manager.launch
setup里面点击TF和Vehicle Model
Map里面TF处点击Ref,加载~/autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/tf_local.launch
,然后点击TF
Computing里面点击lidar_localizer的ndt_mapping
Simulation里面点击Ref加载b.bag,点击play,再点击Pause,点击rviz
选择Fixed Frame为map,添加话题/ndt_map/PointCloud2,size修改为0.1
回到Runtime Manager,点击Pause,等待Playing结束
点击Computing中的lidar_localizer的ndt_mapping的app,再点击Ref选择pcd保存位置,点击PCD OUTPUT,生成a.pcd
相关文章
- echarts数据可视化
- 四层负载均衡的NAT模型与DR模型推导
- Py4DS|3 列表、元组和字典
- HashMap源码解析(一)
- Pycharm开发Django项目MySQL数据库
- 系统是如何给你匹配瓜皮队友的?
- 腾讯云Elasticsearch索引生命周期管理原理及实践
- 第3节:K邻近法原理即numpy实现版
- 从一个简单的Delete删数据场景谈TiDB数据库开发规范的重要性
- 大数据 | HDFS 元数据持久化笔记
- 比较copykat和infercnv这两个从单细胞转录组数据推断肿瘤拷贝数变异技术差异
- 单细胞转录组鉴定与骨关节炎相关的关键基因和通路
- RUNX1在B前体急性白血病细胞中的过表达(readMM函数活学活用)
- 面试官说:你来设计一个短链接生成系统吧
- Spring Boot集成数据源
- 一文读懂MongoDB事务处理
- HTTP/2:HTTP/1.1你该进步了
- 前端编程-大气模拟计算之预报查询
- 数据科学:Sklearn中的决策树,底层是如何设计和存储的?
- MySQL内存到底消耗在哪里?