From 405959c07ab4045eabd9c04c3e4376e937ee1281 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Tue, 7 Feb 2023 14:07:26 -0800 Subject: [PATCH 1/3] private parameters and generalization --- config/params.yaml | 62 +++++----- include/patchworkpp/patchworkpp.hpp | 172 ++++++++++++++-------------- launch/demo.launch | 12 +- src/demo.cpp | 22 ++-- 4 files changed, 134 insertions(+), 134 deletions(-) diff --git a/config/params.yaml b/config/params.yaml index f783e10..9cd6221 100755 --- a/config/params.yaml +++ b/config/params.yaml @@ -1,36 +1,36 @@ save_flag: true -patchworkpp: - - sensor_height: 1.723 +# patchworkpp: - mode: "czm" - verbose: false # To check effect of uprightness/elevation/flatness - visualize: true # Ground Likelihood Estimation is visualized - - # Ground Plane Fitting parameters - num_iter: 3 # Number of iterations for ground plane estimation using PCA. - num_lpr: 20 # Maximum number of points to be selected as lowest points representative. - num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. - th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. - th_dist: 0.125 # threshold for thickenss of ground. - th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. - th_dist_v: 0.1 # threshold for thickenss of vertical structure. - max_r: 80.0 # max_range of ground estimation area - min_r: 2.7 # min_range of ground estimation area - uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. +sensor_height: 1.723 - adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered - czm: - num_zones: 4 - num_sectors_each_zone: [16, 32, 54, 32] - mum_rings_each_zone: [2, 4, 4, 4] - elevation_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. - flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. +mode: "czm" +verbose: false # To check effect of uprightness/elevation/flatness +visualize: true # Ground Likelihood Estimation is visualized - enable_RNR : true - enable_RVPF : true - enable_TGR : true - - RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. - RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. \ No newline at end of file +# Ground Plane Fitting parameters +num_iter: 3 # Number of iterations for ground plane estimation using PCA. +num_lpr: 20 # Maximum number of points to be selected as lowest points representative. +num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. +th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. +th_dist: 0.125 # threshold for thickenss of ground. +th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. +th_dist_v: 0.1 # threshold for thickenss of vertical structure. +max_r: 80.0 # max_range of ground estimation area +min_r: 2.7 # min_range of ground estimation area +uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. + +adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered +czm: + num_zones: 4 + num_sectors_each_zone: [16, 32, 54, 32] + mum_rings_each_zone: [2, 4, 4, 4] + elevation_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. + flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. + +enable_RNR : true +enable_RVPF : true +enable_TGR : true + +RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. +RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index 2b7ce33..7d5c4bf 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -1,12 +1,12 @@ /** * @file patchworkpp.hpp * @author Seungjae Lee - * @brief + * @brief * @version 0.1 * @date 2022-07-20 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef PATCHWORKPP_H #define PATCHWORKPP_H @@ -43,15 +43,15 @@ template bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; } template -struct RevertCandidate -{ +struct RevertCandidate +{ int concentric_idx; int sector_idx; double ground_flatness; double line_variable; Eigen::Vector4f pc_mean; pcl::PointCloud regionwise_ground; - + RevertCandidate(int _c_idx, int _s_idx, double _flatness, double _line_var, Eigen::Vector4f _pc_mean, pcl::PointCloud _ground) : concentric_idx(_c_idx), sector_idx(_s_idx), ground_flatness(_flatness), line_variable(_line_var), pc_mean(_pc_mean), regionwise_ground(_ground) {} }; @@ -69,27 +69,27 @@ class PatchWorkpp { // Init ROS related ROS_INFO("Inititalizing PatchWork++..."); - node_handle_.param("/patchworkpp/verbose", verbose_, false); - - node_handle_.param("/patchworkpp/sensor_height", sensor_height_, 1.723); - node_handle_.param("/patchworkpp/num_iter", num_iter_, 3); - node_handle_.param("/patchworkpp/num_lpr", num_lpr_, 20); - node_handle_.param("/patchworkpp/num_min_pts", num_min_pts_, 10); - node_handle_.param("/patchworkpp/th_seeds", th_seeds_, 0.4); - node_handle_.param("/patchworkpp/th_dist", th_dist_, 0.3); - node_handle_.param("/patchworkpp/th_seeds_v", th_seeds_v_, 0.4); - node_handle_.param("/patchworkpp/th_dist_v", th_dist_v_, 0.3); - node_handle_.param("/patchworkpp/max_r", max_range_, 80.0); - node_handle_.param("/patchworkpp/min_r", min_range_, 2.7); - node_handle_.param("/patchworkpp/uprightness_thr", uprightness_thr_, 0.5); - node_handle_.param("/patchworkpp/adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); - node_handle_.param("/patchworkpp/RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); - node_handle_.param("/patchworkpp/RNR_intensity_thr", RNR_intensity_thr_, 0.2); - node_handle_.param("/patchworkpp/max_flatness_storage", max_flatness_storage_, 1000); - node_handle_.param("/patchworkpp/max_elevation_storage", max_elevation_storage_, 1000); - node_handle_.param("/patchworkpp/enable_RNR", enable_RNR_, true); - node_handle_.param("/patchworkpp/enable_RVPF", enable_RVPF_, true); - node_handle_.param("/patchworkpp/enable_TGR", enable_TGR_, true); + node_handle_.param("verbose", verbose_, false); + + node_handle_.param("sensor_height", sensor_height_, 1.723); + node_handle_.param("num_iter", num_iter_, 3); + node_handle_.param("num_lpr", num_lpr_, 20); + node_handle_.param("num_min_pts", num_min_pts_, 10); + node_handle_.param("th_seeds", th_seeds_, 0.4); + node_handle_.param("th_dist", th_dist_, 0.3); + node_handle_.param("th_seeds_v", th_seeds_v_, 0.4); + node_handle_.param("th_dist_v", th_dist_v_, 0.3); + node_handle_.param("max_r", max_range_, 80.0); + node_handle_.param("min_r", min_range_, 2.7); + node_handle_.param("uprightness_thr", uprightness_thr_, 0.5); + node_handle_.param("adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); + node_handle_.param("RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); + node_handle_.param("RNR_intensity_thr", RNR_intensity_thr_, 0.2); + node_handle_.param("max_flatness_storage", max_flatness_storage_, 1000); + node_handle_.param("max_elevation_storage", max_elevation_storage_, 1000); + node_handle_.param("enable_RNR", enable_RNR_, true); + node_handle_.param("enable_RVPF", enable_RVPF_, true); + node_handle_.param("enable_TGR", enable_TGR_, true); ROS_INFO("Sensor Height: %f", sensor_height_); ROS_INFO("Num of Iteration: %d", num_iter_); @@ -103,11 +103,11 @@ class PatchWorkpp { ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); // CZM denotes 'Concentric Zone Model'. Please refer to our paper - node_handle_.getParam("/patchworkpp/czm/num_zones", num_zones_); - node_handle_.getParam("/patchworkpp/czm/num_sectors_each_zone", num_sectors_each_zone_); - node_handle_.getParam("/patchworkpp/czm/mum_rings_each_zone", num_rings_each_zone_); - node_handle_.getParam("/patchworkpp/czm/elevation_thresholds", elevation_thr_); - node_handle_.getParam("/patchworkpp/czm/flatness_thresholds", flatness_thr_); + node_handle_.getParam("czm/num_zones", num_zones_); + node_handle_.getParam("czm/num_sectors_each_zone", num_sectors_each_zone_); + node_handle_.getParam("czm/mum_rings_each_zone", num_rings_each_zone_); + node_handle_.getParam("czm/elevation_thresholds", elevation_thr_); + node_handle_.getParam("czm/flatness_thresholds", flatness_thr_); ROS_INFO("Num. zones: %d", num_zones_); @@ -133,7 +133,7 @@ class PatchWorkpp { flatness_thr_[3]).str() << endl; num_rings_of_interest_ = elevation_thr_.size(); - node_handle_.param("/patchworkpp/visualize", visualize_, true); + node_handle_.param("visualize", visualize_, true); int num_polygons = std::inner_product(num_rings_each_zone_.begin(), num_rings_each_zone_.end(), num_sectors_each_zone_.begin(), 0); poly_list_.header.frame_id = "map"; @@ -144,12 +144,12 @@ class PatchWorkpp { regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - PlaneViz = node_handle_.advertise("/patchworkpp/plane", 100, true); - pub_revert_pc = node_handle_.advertise("/patchworkpp/revert_pc", 100, true); - pub_reject_pc = node_handle_.advertise("/patchworkpp/reject_pc", 100, true); - pub_normal = node_handle_.advertise("/patchworkpp/normals", 100, true); - pub_noise = node_handle_.advertise("/patchworkpp/noise", 100, true); - pub_vertical = node_handle_.advertise("/patchworkpp/vertical", 100, true); + PlaneViz = node_handle_.advertise("plane", 100, true); + pub_revert_pc = node_handle_.advertise("revert_pc", 100, true); + pub_reject_pc = node_handle_.advertise("reject_pc", 100, true); + pub_normal = node_handle_.advertise("normals", 100, true); + pub_noise = node_handle_.advertise("noise", 100, true); + pub_vertical = node_handle_.advertise("vertical", 100, true); min_range_z2_ = (7 * min_range_ + max_range_) / 8.0; min_range_z3_ = (3 * min_range_ + max_range_) / 4.0; @@ -244,7 +244,7 @@ class PatchWorkpp { pcl::PointCloud revert_pc_, reject_pc_, noise_pc_, vertical_pc_; pcl::PointCloud ground_pc_; - pcl::PointCloud normals_; + pcl::PointCloud normals_; pcl::PointCloud regionwise_ground_, regionwise_nonground_; @@ -256,11 +256,11 @@ class PatchWorkpp { void pc2czm(const pcl::PointCloud &src, std::vector &czm, pcl::PointCloud &cloud_nonground); void reflected_noise_removal(pcl::PointCloud &cloud, pcl::PointCloud &cloud_nonground); - + void temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, std::vector ring_flatness, std::vector> candidates, int concentric_idx); - + void calc_mean_stdev(std::vector vec, double &mean, double &stdev); void update_elevation_thr(); @@ -410,7 +410,7 @@ void PatchWorkpp::extract_initial_seeds( } } } - + // Calculate the mean height value. for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { sum += p_sorted.points[i].z; @@ -429,12 +429,12 @@ void PatchWorkpp::extract_initial_seeds( template inline void PatchWorkpp::reflected_noise_removal(pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_nonground) { - for (int i=0; i::reflected_noise_removal(pcl::PointCloud &cloud noise_idxs_.push(i); } } - + if (verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl; } @@ -458,7 +458,7 @@ void PatchWorkpp::estimate_ground( pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, double &time_taken) { - + unique_lock lock(mutex_); poly_list_.header.stamp = ros::Time::now(); @@ -473,7 +473,7 @@ void PatchWorkpp::estimate_ground( double t_total_estimate = 0.0; start = ros::Time::now().toSec(); - + cloud_ground.clear(); cloud_nonground.clear(); @@ -487,21 +487,21 @@ void PatchWorkpp::estimate_ground( pc2czm(cloud_in, ConcentricZoneModel_, cloud_nonground); t2 = ros::Time::now().toSec(); - + int concentric_idx = 0; double t_sort = 0; std::vector> candidates; std::vector ringwise_flatness; - + for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) { - + auto zone = ConcentricZoneModel_[zone_idx]; for (int ring_idx = 0; ring_idx < num_rings_each_zone_[zone_idx]; ++ring_idx) { for (int sector_idx = 0; sector_idx < num_sectors_each_zone_[zone_idx]; ++sector_idx) { - + if (zone[ring_idx][sector_idx].points.size() < num_min_pts_) { cloud_nonground += zone[ring_idx][sector_idx]; @@ -510,16 +510,16 @@ void PatchWorkpp::estimate_ground( // --------- region-wise sorting (faster than global sorting method) ---------------- // double t_sort_0 = ros::Time::now().toSec(); - + sort(zone[ring_idx][sector_idx].points.begin(), zone[ring_idx][sector_idx].points.end(), point_z_cmp); - + double t_sort_1 = ros::Time::now().toSec(); t_sort += (t_sort_1 - t_sort_0); // ---------------------------------------------------------------------------------- // double t_tmp0 = ros::Time::now().toSec(); extract_piecewiseground(zone_idx, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_); - + double t_tmp1 = ros::Time::now().toSec(); t_total_ground += t_tmp1 - t_tmp0; pca_time_ += t_tmp1 - t_tmp0; @@ -530,7 +530,7 @@ void PatchWorkpp::estimate_ground( const double ground_elevation = pc_mean_(2, 0); const double ground_flatness = singular_values_.minCoeff(); const double line_variable = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits::max(); - + double heading = 0.0; for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i); @@ -552,14 +552,14 @@ void PatchWorkpp::estimate_ground( double t_tmp2 = ros::Time::now().toSec(); - /* + /* About 'is_heading_outside' condidition, heading should be smaller than 0 theoretically. - ( Imagine the geometric relationship between the surface normal vector on the ground plane and + ( Imagine the geometric relationship between the surface normal vector on the ground plane and the vector connecting the sensor origin and the mean point of the ground plane ) - However, when the patch is far awaw from the sensor origin, + However, when the patch is far awaw from the sensor origin, heading could be larger than 0 even if it's ground due to lack of amount of ground plane points. - + Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition ) */ bool is_upright = ground_uprightness > uprightness_thr_; @@ -636,13 +636,13 @@ void PatchWorkpp::estimate_ground( concentric_idx++; } - } + } double t_update = ros::Time::now().toSec(); update_elevation_thr(); update_flatness_thr(); - + end = ros::Time::now().toSec(); time_taken = end - start; @@ -686,7 +686,7 @@ void PatchWorkpp::estimate_ground( { PlaneViz.publish(poly_list_); } - + revert_pc_.clear(); reject_pc_.clear(); normals_.clear(); @@ -714,14 +714,14 @@ void PatchWorkpp::update_elevation_thr(void) int exceed_num = update_elevation_[i].size() - max_elevation_storage_; if (exceed_num > 0) update_elevation_[i].erase(update_elevation_[i].begin(), update_elevation_[i].begin() + exceed_num); } - + if (verbose_) { cout << "sensor height: " << sensor_height_ << endl; cout << (boost::format("elevation_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") % elevation_thr_[0] % elevation_thr_[1] % elevation_thr_[2] % elevation_thr_[3]).str() << endl; } - + return; } @@ -742,13 +742,13 @@ void PatchWorkpp::update_flatness_thr(void) int exceed_num = update_flatness_[i].size() - max_flatness_storage_; if (exceed_num > 0) update_flatness_[i].erase(update_flatness_[i].begin(), update_flatness_[i].begin() + exceed_num); } - + if (verbose_) { - cout << (boost::format("flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") + cout << (boost::format("flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") % flatness_thr_[0] % flatness_thr_[1] % flatness_thr_[2] % flatness_thr_[3]).str() << endl; } - + return; } @@ -761,7 +761,7 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ double mean_flatness = 0.0, stdev_flatness = 0.0; calc_mean_stdev(ring_flatness, mean_flatness, stdev_flatness); - + if (verbose_) { cout << "[" << candidates[0].concentric_idx << ", " << candidates[0].sector_idx << "]" @@ -771,14 +771,14 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ for( size_t i=0; i candidate = candidates[i]; - + // Debug if(verbose_) { cout << "\033[1;33m" << candidate.sector_idx << "th flat_sector_candidate" << " / flatness: " << candidate.ground_flatness << " / line_variable: " << candidate.line_variable - << " / ground_num : " << candidate.regionwise_ground.size() + << " / ground_num : " << candidate.regionwise_ground.size() << "\033[0m" << endl; } @@ -788,7 +788,7 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < th_dist_*th_dist_) prob_flatness = 1.0; double prob_line = 1.0; - if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)// candidate.ground_elevation > elevation_thr_[concentric_idx]) + if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)// candidate.ground_elevation > elevation_thr_[concentric_idx]) { // if (verbose_) cout << "line_dir: " << candidate.line_dir << endl; prob_line = 0.0; @@ -804,19 +804,19 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ { cout << "\033[1;32m" << "REVERT TRUE" << "\033[0m" << endl; } - + revert_pc_ += candidate.regionwise_ground; cloud_ground += candidate.regionwise_ground; } else { - if (verbose_) + if (verbose_) { cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl; } reject_pc_ += candidate.regionwise_ground; cloud_nonground += candidate.regionwise_ground; - } + } } } @@ -829,13 +829,13 @@ void PatchWorkpp::extract_piecewiseground( const int zone_idx, const pcl::PointCloud &src, pcl::PointCloud &dst, pcl::PointCloud &non_ground_dst) { - + // 0. Initialization if (!ground_pc_.empty()) ground_pc_.clear(); if (!dst.empty()) dst.clear(); if (!non_ground_dst.empty()) non_ground_dst.clear(); - - // 1. Region-wise Vertical Plane Fitting (R-VPF) + + // 1. Region-wise Vertical Plane Fitting (R-VPF) // : removes potential vertical plane under the ground plane pcl::PointCloud src_wo_verticals; src_wo_verticals = src; @@ -852,7 +852,7 @@ void PatchWorkpp::extract_piecewiseground( pcl::PointCloud src_tmp; src_tmp = src_wo_verticals; src_wo_verticals.clear(); - + Eigen::MatrixXf points(src_tmp.points.size(), 3); int j = 0; for (auto &p:src_tmp.points) { @@ -873,7 +873,7 @@ void PatchWorkpp::extract_piecewiseground( else break; } } - + extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_); estimate_plane(ground_pc_); @@ -890,7 +890,7 @@ void PatchWorkpp::extract_piecewiseground( for (int i = 0; i < num_iter_; i++) { ground_pc_.clear(); - + // ground plane model Eigen::VectorXf result = points * normal_; // threshold filter @@ -994,7 +994,7 @@ void PatchWorkpp::calc_mean_stdev(std::vector vec, double &mean, mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size(); - for (int i=0; i inline void PatchWorkpp::pc2czm(const pcl::PointCloud &src, std::vector &czm, pcl::PointCloud &cloud_nonground) { for (int i=0; i::pc2czm(const pcl::PointCloud &src, std::vector double r = xy2radius(pt.x, pt.y); if ((r <= max_range_) && (r > min_range_)) { double theta = xy2theta(pt.x, pt.y); - + int zone_idx = 0; if ( r < min_ranges_[1] ) zone_idx = 0; else if ( r < min_ranges_[2] ) zone_idx = 1; else if ( r < min_ranges_[3] ) zone_idx = 2; else zone_idx = 3; - + int ring_idx = min(static_cast(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1); int sector_idx = min(static_cast((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1); - + czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt); } else { diff --git a/launch/demo.launch b/launch/demo.launch index c724c01..b3b6e42 100755 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -1,12 +1,12 @@ - - - - + -"/kitti/velo/pointcloud" + + + + - + diff --git a/src/demo.cpp b/src/demo.cpp index 5eb06eb..eaec72d 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -33,7 +33,7 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) pcl::PointCloud pc_curr; pcl::PointCloud pc_ground; pcl::PointCloud pc_non_ground; - + pcl::fromROSMsg(*cloud_msg, pc_curr); PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); @@ -41,26 +41,26 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) cout << "\033[1;32m" << "Result: Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() << " (running_time: " << time_taken << " sec)" << "\033[0m" << endl; - pub_cloud.publish(cloud2msg(pc_curr)); - pub_ground.publish(cloud2msg(pc_ground)); - pub_non_ground.publish(cloud2msg(pc_non_ground)); + pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.frame_id)); + pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.frame_id)); + pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.frame_id)); } int main(int argc, char**argv) { ros::init(argc, argv, "Demo"); - ros::NodeHandle nh; + ros::NodeHandle nh("~"); std::string cloud_topic; - nh.param("/cloud_topic", cloud_topic, "/pointcloud"); + nh.param("cloud_topic", cloud_topic, "/pointcloud"); cout << "Operating patchwork++..." << endl; PatchworkppGroundSeg.reset(new PatchWorkpp(&nh)); - pub_cloud = nh.advertise("/demo/cloud", 100, true); - pub_ground = nh.advertise("/demo/ground", 100, true); - pub_non_ground = nh.advertise("/demo/nonground", 100, true); - + pub_cloud = nh.advertise("cloud", 100, true); + pub_ground = nh.advertise("ground", 100, true); + pub_non_ground = nh.advertise("nonground", 100, true); + ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud); @@ -68,6 +68,6 @@ int main(int argc, char**argv) { { ros::spinOnce(); } - + return 0; } From bf274dd1fb653e678ae9bd96bfaa920fd8b2959e Mon Sep 17 00:00:00 2001 From: Will Baker Date: Tue, 7 Feb 2023 14:54:14 -0800 Subject: [PATCH 2/3] cloud not on private node handle --- include/patchworkpp/patchworkpp.hpp | 11 ++++++----- src/demo.cpp | 13 +++++++------ 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index 7d5c4bf..010d6a2 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -462,6 +462,7 @@ void PatchWorkpp::estimate_ground( unique_lock lock(mutex_); poly_list_.header.stamp = ros::Time::now(); + poly_list_.header.frame_id = cloud_in.header.frame_id; if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); @@ -658,27 +659,27 @@ void PatchWorkpp::estimate_ground( sensor_msgs::PointCloud2 cloud_ROS; pcl::toROSMsg(revert_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); - cloud_ROS.header.frame_id = "map"; + cloud_ROS.header.frame_id = cloud_in.header.frame_id; pub_revert_pc.publish(cloud_ROS); pcl::toROSMsg(reject_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); - cloud_ROS.header.frame_id = "map"; + cloud_ROS.header.frame_id = cloud_in.header.frame_id; pub_reject_pc.publish(cloud_ROS); pcl::toROSMsg(normals_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); - cloud_ROS.header.frame_id = "map"; + cloud_ROS.header.frame_id = cloud_in.header.frame_id; pub_normal.publish(cloud_ROS); pcl::toROSMsg(noise_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); - cloud_ROS.header.frame_id = "map"; + cloud_ROS.header.frame_id = cloud_in.header.frame_id; pub_noise.publish(cloud_ROS); pcl::toROSMsg(vertical_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); - cloud_ROS.header.frame_id = "map"; + cloud_ROS.header.frame_id = cloud_in.header.frame_id; pub_vertical.publish(cloud_ROS); } diff --git a/src/demo.cpp b/src/demo.cpp index eaec72d..3c28acd 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -49,17 +49,18 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) int main(int argc, char**argv) { ros::init(argc, argv, "Demo"); - ros::NodeHandle nh("~"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); std::string cloud_topic; - nh.param("cloud_topic", cloud_topic, "/pointcloud"); + pnh.param("cloud_topic", cloud_topic, "/pointcloud"); cout << "Operating patchwork++..." << endl; - PatchworkppGroundSeg.reset(new PatchWorkpp(&nh)); + PatchworkppGroundSeg.reset(new PatchWorkpp(&pnh)); - pub_cloud = nh.advertise("cloud", 100, true); - pub_ground = nh.advertise("ground", 100, true); - pub_non_ground = nh.advertise("nonground", 100, true); + pub_cloud = pnh.advertise("cloud", 100, true); + pub_ground = pnh.advertise("ground", 100, true); + pub_non_ground = pnh.advertise("nonground", 100, true); ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud); From fa4341c1e20f3eaf20a0296c870accd5cc093174 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Tue, 7 Feb 2023 15:21:54 -0800 Subject: [PATCH 3/3] install --- CMakeLists.txt | 15 +++++++++++++++ package.xml | 5 +++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fb0703e..edaedb1 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,3 +72,18 @@ add_dependencies(demo patchworkpp_generate_messages_cpp) add_executable(video src/video.cpp) target_link_libraries(video ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(video patchworkpp_generate_messages_cpp) + + +# ==== Install ==== +install(TARGETS demo + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/package.xml b/package.xml index 3c77bb8..ed6916a 100755 --- a/package.xml +++ b/package.xml @@ -18,8 +18,8 @@ rospy std_msgs nav_msgs - sensor_msgs - jsk_recognition_msgs + sensor_msgs + jsk_recognition_msgs roscpp rospy std_msgs @@ -29,6 +29,7 @@ message_generation sensor_msgs jsk_recognition_msgs + laser_geometry