From 79c57995688fcc965d2d45f385638415e9f922ab Mon Sep 17 00:00:00 2001 From: Achmad Fathoni Date: Thu, 15 Apr 2021 19:13:47 +0700 Subject: [PATCH 1/3] Remove bool argument from graph->load() --- src/hdl_graph_slam/graph_slam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hdl_graph_slam/graph_slam.cpp b/src/hdl_graph_slam/graph_slam.cpp index d6a2dcd7..a20b9950 100644 --- a/src/hdl_graph_slam/graph_slam.cpp +++ b/src/hdl_graph_slam/graph_slam.cpp @@ -334,7 +334,7 @@ bool GraphSLAM::load(const std::string& filename) { g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); std::ifstream ifs(filename); - if(!graph->load(ifs, true)) { + if(!graph->load(ifs)) { return false; } From 8e246aa0e5e5a2d36818b4a72e58f718b563a2b8 Mon Sep 17 00:00:00 2001 From: Achmad Fathoni Date: Thu, 15 Apr 2021 19:23:59 +0700 Subject: [PATCH 2/3] Change publish argument to variable that pointed --- apps/floor_detection_nodelet.cpp | 4 ++-- apps/prefiltering_nodelet.cpp | 4 ++-- apps/scan_matching_odometry_nodelet.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/apps/floor_detection_nodelet.cpp b/apps/floor_detection_nodelet.cpp index 93f9435a..d3f31ef6 100644 --- a/apps/floor_detection_nodelet.cpp +++ b/apps/floor_detection_nodelet.cpp @@ -124,7 +124,7 @@ class FloorDetectionNodelet : public nodelet::Nodelet { if(floor_filtered_pub.getNumSubscribers()) { filtered->header = cloud->header; - floor_filtered_pub.publish(filtered); + floor_filtered_pub.publish(*filtered); } // too few points for RANSAC @@ -171,7 +171,7 @@ class FloorDetectionNodelet : public nodelet::Nodelet { extract.filter(*inlier_cloud); inlier_cloud->header = cloud->header; - floor_points_pub.publish(inlier_cloud); + floor_points_pub.publish(*inlier_cloud); } return Eigen::Vector4f(coeffs); diff --git a/apps/prefiltering_nodelet.cpp b/apps/prefiltering_nodelet.cpp index ce8fd96c..b51353a1 100644 --- a/apps/prefiltering_nodelet.cpp +++ b/apps/prefiltering_nodelet.cpp @@ -131,7 +131,7 @@ class PrefilteringNodelet : public nodelet::Nodelet { filtered = downsample(filtered); filtered = outlier_removal(filtered); - points_pub.publish(filtered); + points_pub.publish(*filtered); } pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { @@ -200,7 +200,7 @@ class PrefilteringNodelet : public nodelet::Nodelet { colored->at(i).g = 128; colored->at(i).b = 255 * (1 - t); } - colored_pub.publish(colored); + colored_pub.publish(*colored); } sensor_msgs::ImuConstPtr imu_msg = imu_queue.front(); diff --git a/apps/scan_matching_odometry_nodelet.cpp b/apps/scan_matching_odometry_nodelet.cpp index 4215c1d0..d3dff75c 100644 --- a/apps/scan_matching_odometry_nodelet.cpp +++ b/apps/scan_matching_odometry_nodelet.cpp @@ -254,7 +254,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet { { pcl::transformPointCloud (*cloud, *aligned, odom); aligned->header.frame_id=odom_frame_id; - aligned_points_pub.publish(aligned); + aligned_points_pub.publish(*aligned); } return odom; From 52a78017059a51c261a48dc659c5a14f1e22e4c3 Mon Sep 17 00:00:00 2001 From: Achmad Fathoni Date: Sat, 17 Apr 2021 08:38:31 +0700 Subject: [PATCH 3/3] Change cloud_callback signature --- apps/prefiltering_nodelet.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/prefiltering_nodelet.cpp b/apps/prefiltering_nodelet.cpp index b51353a1..268c319c 100644 --- a/apps/prefiltering_nodelet.cpp +++ b/apps/prefiltering_nodelet.cpp @@ -103,7 +103,8 @@ class PrefilteringNodelet : public nodelet::Nodelet { imu_queue.push_back(imu_msg); } - void cloud_callback(pcl::PointCloud::ConstPtr src_cloud) { + void cloud_callback(const pcl::PointCloud& src_cloud_r) { + pcl::PointCloud::ConstPtr src_cloud = src_cloud_r.makeShared(); if(src_cloud->empty()) { return; }