From 3ee504281459b53d5893842eeb0435d581dfa2c3 Mon Sep 17 00:00:00 2001 From: Kenny Chen Date: Mon, 20 Jun 2022 11:23:01 -0700 Subject: [PATCH] ROS service to save generated map into a .pcd --- CMakeLists.txt | 9 +++++++++ README.md | 7 +++++++ cfg/dlo.yaml | 2 +- include/dlo/dlo.h | 1 + include/dlo/map.h | 5 +++++ package.xml | 5 ++++- src/dlo/map.cc | 42 ++++++++++++++++++++++++++++++++++++++---- srv/save_pcd.srv | 4 ++++ 8 files changed, 69 insertions(+), 6 deletions(-) create mode 100644 srv/save_pcd.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a0e2e7..f4082ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,8 +40,17 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs geometry_msgs pcl_ros + message_generation ) +add_service_files( + DIRECTORY srv + FILES + save_pcd.srv +) + +generate_messages() + catkin_package( CATKIN_DEPENDS roscpp diff --git a/README.md b/README.md index eed5ac9..5ea48bd 100644 --- a/README.md +++ b/README.md @@ -60,6 +60,13 @@ If successful, RViz will open and you will see similar terminal outputs to the f drawing

+### Services +To save DLO's generated map into `.pcd` format, call the following service: + +``` +rosservice call /robot/dlo_map/save_pcd LEAF_SIZE SAVE_PATH +``` + ### Test Data For your convenience, we provide example test data [here](https://ucla.box.com/shared/static/ziojd3auzp0zzcgwb1ucau9anh69xwv9.bag) (9 minutes, ~4.2GB). To run, first launch DLO (with default point cloud and IMU topics) via: diff --git a/cfg/dlo.yaml b/cfg/dlo.yaml index 62d5b10..0c57740 100644 --- a/cfg/dlo.yaml +++ b/cfg/dlo.yaml @@ -9,7 +9,7 @@ dlo: - version: 1.3.1 + version: 1.4.0 adaptiveParams: true diff --git a/include/dlo/dlo.h b/include/dlo/dlo.h index 79ad55a..f1d8b9d 100644 --- a/include/dlo/dlo.h +++ b/include/dlo/dlo.h @@ -45,6 +45,7 @@ #include #include +#include #include typedef pcl::PointXYZI PointType; diff --git a/include/dlo/map.h b/include/dlo/map.h index b7ce82e..1102000 100644 --- a/include/dlo/map.h +++ b/include/dlo/map.h @@ -30,6 +30,9 @@ class dlo::MapNode { void keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe); + bool savePcd(direct_lidar_odometry::save_pcd::Request& req, + direct_lidar_odometry::save_pcd::Response& res); + void getParams(); ros::NodeHandle nh; @@ -39,6 +42,8 @@ class dlo::MapNode { ros::Subscriber keyframe_sub; ros::Publisher map_pub; + ros::ServiceServer save_pcd_srv; + pcl::PointCloud::Ptr dlo_map; pcl::VoxelGrid voxelgrid; diff --git a/package.xml b/package.xml index fcc51c3..e97de6a 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,7 @@ direct_lidar_odometry - 1.3.1 + 1.4.0 Direct LiDAR Odometry: Fast Localization with Dense Point Clouds Kenny J. Chen @@ -16,6 +16,9 @@ catkin + message_generation + message_runtime + roscpp std_msgs sensor_msgs diff --git a/src/dlo/map.cc b/src/dlo/map.cc index 9ed6b2e..a795d9d 100644 --- a/src/dlo/map.cc +++ b/src/dlo/map.cc @@ -26,6 +26,8 @@ dlo::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) { this->keyframe_sub = this->nh.subscribe("keyframes", 1, &dlo::MapNode::keyframeCB, this); this->map_pub = this->nh.advertise("map", 1); + this->save_pcd_srv = this->nh.advertiseService("save_pcd", &dlo::MapNode::savePcd, this); + // initialize map this->dlo_map = pcl::PointCloud::Ptr (new pcl::PointCloud); @@ -99,10 +101,6 @@ void dlo::MapNode::abortTimerCB(const ros::TimerEvent& e) { void dlo::MapNode::publishTimerCB(const ros::TimerEvent& e) { - this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_); - this->voxelgrid.setInputCloud(this->dlo_map); - this->voxelgrid.filter(*this->dlo_map); - if (this->dlo_map->points.size() == this->dlo_map->width * this->dlo_map->height) { sensor_msgs::PointCloud2 map_ros; pcl::toROSMsg(*this->dlo_map, map_ros); @@ -124,8 +122,44 @@ void dlo::MapNode::keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe) pcl::PointCloud::Ptr keyframe_pcl = pcl::PointCloud::Ptr (new pcl::PointCloud); pcl::fromROSMsg(*keyframe, *keyframe_pcl); + // voxel filter + this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_); + this->voxelgrid.setInputCloud(keyframe_pcl); + this->voxelgrid.filter(*keyframe_pcl); + // save keyframe to map this->map_stamp = keyframe->header.stamp; *this->dlo_map += *keyframe_pcl; } + +bool dlo::MapNode::savePcd(direct_lidar_odometry::save_pcd::Request& req, + direct_lidar_odometry::save_pcd::Response& res) { + + pcl::PointCloud::Ptr m = + pcl::PointCloud::Ptr (boost::make_shared>(*this->dlo_map)); + + float leaf_size = req.leaf_size; + std::string p = req.save_path; + + std::cout << std::setprecision(2) << "Saving map to " << p + "/dlo_map.pcd" << "... "; std::cout.flush(); + + // voxelize map + pcl::VoxelGrid vg; + vg.setLeafSize(leaf_size, leaf_size, leaf_size); + vg.setInputCloud(m); + vg.filter(*m); + + // save map + int ret = pcl::io::savePCDFileBinary(p + "/dlo_map.pcd", *m); + res.success = ret == 0; + + if (res.success) { + std::cout << "done" << std::endl; + } else { + std::cout << "failed" << std::endl; + } + + return res.success; + +} diff --git a/srv/save_pcd.srv b/srv/save_pcd.srv new file mode 100644 index 0000000..4692476 --- /dev/null +++ b/srv/save_pcd.srv @@ -0,0 +1,4 @@ +float32 leaf_size +string save_path +--- +bool success \ No newline at end of file