From 18563ed532f7489ad8fb4fca511c0615785e3d82 Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Thu, 25 May 2023 14:12:47 +0200 Subject: [PATCH 1/8] compiles with dynamic reconfigure --- CMakeLists.txt | 9 + cfg/patchwork.cfg | 44 +++++ include/patchworkpp/params.hpp | 169 +++++++++++++++++++ include/patchworkpp/patchworkpp.hpp | 253 ++++++++++++++++------------ launch/demo.launch | 2 +- {config => params}/params.yaml | 6 +- src/demo.cpp | 5 +- src/demo_reconfigure.cpp | 90 ++++++++++ src/offline_kitti.cpp | 4 +- src/video.cpp | 5 +- 10 files changed, 467 insertions(+), 120 deletions(-) create mode 100644 cfg/patchwork.cfg create mode 100644 include/patchworkpp/params.hpp rename {config => params}/params.yaml (93%) create mode 100644 src/demo_reconfigure.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index edaedb1..c9a335e 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS laser_geometry sensor_msgs message_generation + dynamic_reconfigure jsk_recognition_msgs ) @@ -48,6 +49,10 @@ if (OPENMP_FOUND) set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") endif() +generate_dynamic_reconfigure_options( + cfg/patchwork.cfg +) + catkin_package( INCLUDE_DIRS LIBRARIES @@ -69,6 +74,10 @@ add_executable(demo src/demo.cpp) target_link_libraries(demo ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(demo patchworkpp_generate_messages_cpp) +add_executable(demo_reconfigure src/demo_reconfigure.cpp) +target_link_libraries(demo_reconfigure ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) +add_dependencies(demo_reconfigure 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) diff --git a/cfg/patchwork.cfg b/cfg/patchwork.cfg new file mode 100644 index 0000000..f5fb8da --- /dev/null +++ b/cfg/patchwork.cfg @@ -0,0 +1,44 @@ +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# save_flag +gen.add("save_flag", bool_t, 0, "Flag to save the configuration file", True) + +# patchworkpp +gen.add("sensor_height", double_t, 0, "Height of the sensor from the ground", 1.723, 0.0, 100.0) +gen.add("mode", str_t, 0, "Mode of operation", "czm") +gen.add("verbose", bool_t, 0, "Verbose mode for debugging", False) +gen.add("visualize", bool_t, 0, "Flag to visualize the ground likelihood estimation", True) + +# Ground Plane Fitting parameters +gen.add("num_iter", int_t, 0, "Number of iterations for ground plane estimation using PCA", 3, 0, 10) +gen.add("num_lpr", int_t, 0, "Maximum number of points to be selected as lowest points representative", 20, 0, 100) +gen.add("num_min_pts", int_t, 0, "Minimum number of points to be estimated as ground plane in each patch", 10, 0, 100) +gen.add("th_seeds", double_t, 0, "Threshold for lowest point representatives using in initial seeds selection of ground points", 0.3, 0.0, 1.0) +gen.add("th_dist", double_t, 0, "Threshold for thickness of ground", 0.125, 0.0, 1.0) +gen.add("th_seeds_v", double_t, 0, "Threshold for lowest point representatives using in initial seeds selection of vertical structural points", 0.25, 0.0, 1.0) +gen.add("th_dist_v", double_t, 0, "Threshold for thickness of vertical structure", 0.1, 0.0, 1.0) +gen.add("max_r", double_t, 0, "Maximum range of ground estimation area", 80.0, 0.0, 1000.0) +gen.add("min_r", double_t, 0, "Minimum range of ground estimation area", 2.7, 0.0, 100.0) +gen.add("uprightness_thr", double_t, 0, "Threshold of uprightness using in Ground Likelihood Estimation(GLE)", 0.707, 0.0, 1.0) +gen.add("adaptive_seed_selection_margin", double_t, 0, "The points below the adaptive_seed_selection_margin * sensor_height are filtered", -1.2, -10.0, 10.0) + +# czm +gen.add("czm_num_zones", int_t, 0, "Number of zones in czm mode", 4, 0, 10) +gen.add("czm_num_sectors_each_zone", str_t, 0, "Number of sectors in each zone", "16, 32, 54, 32") +gen.add("czm_num_rings_each_zone", str_t, 0, "Number of rings in each zone", "2, 4, 4, 4") +gen.add("czm_elevation_thresholds", str_t, 0, "Threshold of elevation for each ring using in GLE. Those values are updated adaptively.", "0.0, 0.0, 0.0, 0.0") +gen.add("czm_flatness_thresholds", str_t, 0, "Threshold of flatness for each ring using in GLE. Those values are updated adaptively.", "0.0, 0.0, 0.0, 0.0") + +# enable/disable options +gen.add("enable_RNR", bool_t, 0, "Flag to enable/disable RNR", True) +gen.add("enable_RVPF", bool_t, 0, "Flag to enable/disable RVPF", True) +gen.add("enable_TGR", bool_t, 0, "Flag to enable/disable TGR", True) + +# RNR parameters +gen.add("RNR_ver_angle_thr", double_t, 0, "Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.", -15.0, -90.0, 90.0) +gen.add("RNR_intensity_thr", double_t, 0, "Noise points intensity threshold. The reflected points have relatively small intensity than others.", 0.2, 0.0, 1.0) + +# Generate the .cfg file +exit(gen.generate("patchworkpp", "demo", "Patchworkpp")) \ No newline at end of file diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp new file mode 100644 index 0000000..57231ba --- /dev/null +++ b/include/patchworkpp/params.hpp @@ -0,0 +1,169 @@ +#ifndef PATCHWORKPP_PARAMS +#define PATCHWORKPP_PARAMS + +#include <regex> +#include <vector> +#include <stdexcept> +#include <ros/ros.h> +#include <dynamic_reconfigure/server.h> +#include <patchworkpp/PatchworkppConfig.h> + +class ParamsNh +{ +public: + ParamsNh() { + auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2); + server_.setCallback(f); + validate(); + } + + ParamsNh(const ros::NodeHandle& nh) { + nh.param("verbose", verbose_, false); + nh.param("sensor_height", sensor_height_, 1.723); + nh.param("num_iter", num_iter_, 3); + nh.param("num_lpr", num_lpr_, 20); + nh.param("num_min_pts", num_min_pts_, 10); + nh.param("th_seeds", th_seeds_, 0.4); + nh.param("th_dist", th_dist_, 0.3); + nh.param("th_seeds_v", th_seeds_v_, 0.4); + nh.param("th_dist_v", th_dist_v_, 0.3); + nh.param("max_r", max_range_, 80.0); + nh.param("min_r", min_range_, 2.7); + nh.param("uprightness_thr", uprightness_thr_, 0.5); + nh.param("adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); + nh.param("RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); + nh.param("RNR_intensity_thr", RNR_intensity_thr_, 0.2); + nh.param("max_flatness_storage", max_flatness_storage_, 1000); + nh.param("max_elevation_storage", max_elevation_storage_, 1000); + nh.param("enable_RNR", enable_RNR_, true); + nh.param("enable_RVPF", enable_RVPF_, true); + nh.param("enable_TGR", enable_TGR_, true); + + // CZM denotes 'Concentric Zone Model'. Please refer to our paper + nh.getParam("czm/num_zones", czm.num_zones_); + nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_); + nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_); + nh.getParam("czm/elevation_thresholds", czm.elevation_thresholds_); + nh.getParam("czm/flatness_thresholds", czm.flatness_thresholds_); + + initialized_ = true; + validate(); + } + + void print_params() const { + ROS_INFO("Sensor Height: %f", sensor_height_); + ROS_INFO("Num of Iteration: %d", num_iter_); + ROS_INFO("Num of LPR: %d", num_lpr_); + ROS_INFO("Num of min. points: %d", num_min_pts_); + ROS_INFO("Seeds Threshold: %f", th_seeds_); + ROS_INFO("Distance Threshold: %f", th_dist_); + ROS_INFO("Max. range:: %f", max_range_); + ROS_INFO("Min. range:: %f", min_range_); + ROS_INFO("Normal vector threshold: %f", uprightness_thr_); + ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); + } + + bool validate() { + return check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones and length of num_sectors_each_zone must match") &&\ + check(czm.num_zones_ == czm.num_rings_each_zone_.size(), "num_zones and length of num_rings_each_zone must match") &&\ + check(czm.num_zones_ == czm.elevation_thresholds_.size(), "num_zones and length of elevation_thresholds must match") &&\ + check(czm.num_zones_ == czm.flatness_thresholds_.size(), "num_zones and length of flatness_thresholds must match"); + } + +private: + void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) { + verbose_ = config.verbose; + sensor_height_ = config.sensor_height; + num_iter_ = config.num_iter; + num_lpr_ = config.num_lpr; + num_min_pts_ = config.num_min_pts; + th_seeds_ = config.th_seeds; + th_dist_ = config.th_dist; + th_seeds_v_ = config.th_seeds_v; + th_dist_v_ = config.th_dist_v; + uprightness_thr_ = config.uprightness_thr; + adaptive_seed_selection_margin_ = config.adaptive_seed_selection_margin; + RNR_ver_angle_thr_ = config.RNR_ver_angle_thr; + RNR_intensity_thr_ = config.RNR_intensity_thr; + enable_RNR_ = config.enable_RNR; + enable_RVPF_ = config.enable_RVPF; + enable_TGR_ = config.enable_TGR; + + czm.num_zones_ = config.czm_num_zones; + czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone); + czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone); + czm.elevation_thresholds_ = convert_string_to_vector<double>(config.czm_elevation_thresholds); + czm.flatness_thresholds_ = convert_string_to_vector<double>(config.czm_flatness_thresholds); + + ROS_INFO("Updated params"); + initialized_ = true; + validate(); + } + + + template <typename T> + std::vector<T> convert_string_to_vector(const std::string& str, char separator = ',') { + std::vector<T> result; + std::istringstream iss(str); + std::string token; + + while (std::getline(iss, token, separator)) { + token = std::regex_replace(token, std::regex("\\s+"), ""); + T num; + std::stringstream stream(token); + stream >> num; + if (stream.fail()) { + ROS_WARN_STREAM("Can't convert " << token << " to number"); + return result; + } + } + + return result; + } + + + bool check(bool assertion, std::string description) { + if (not assertion) { + ROS_WARN_STREAM(description); + return false; + } + + return true; + } + + std::string mode_; + bool initialized_; + bool verbose_; + double sensor_height_; + int num_iter_; + int num_lpr_; + int num_min_pts_; + double th_seeds_; + double th_dist_; + double th_seeds_v_; + double th_dist_v_; + double max_range_; + double min_range_; + double uprightness_thr_; + double adaptive_seed_selection_margin_; + double RNR_ver_angle_thr_; + double RNR_intensity_thr_; + int max_flatness_storage_; + int max_elevation_storage_; + bool enable_RNR_; + bool enable_RVPF_; + bool enable_TGR_; + + struct CZM + { + int num_zones_; + std::vector<int> num_sectors_each_zone_; + std::vector<int> num_rings_each_zone_; + std::vector<double> elevation_thresholds_; + std::vector<double> flatness_thresholds_; + } czm; + + dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_; +}; + +#endif \ No newline at end of file diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index f45e249..e3a9df9 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -21,6 +21,7 @@ #include <mutex> #include <patchworkpp/utils.hpp> +#include <patchworkpp/params.hpp> #define MARKER_Z_VALUE -2.2 #define UPRIGHT_ENOUGH 0.55 @@ -63,114 +64,30 @@ class PatchWorkpp { typedef std::vector<pcl::PointCloud<PointT>> Ring; typedef std::vector<Ring> Zone; - PatchWorkpp() {}; + PatchWorkpp() : params_() { + params_.print_params(); - PatchWorkpp(ros::NodeHandle *nh) : node_handle_(*nh) { - // Init ROS related - ROS_INFO("Inititalizing PatchWork++..."); + plane_viz_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); + pub_revert_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); + pub_reject_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true); + pub_normal_ = node_handle_.advertise<sensor_msgs::PointCloud2>("normals", 100, true); + pub_noise_ = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true); + pub_vertical_ = node_handle_.advertise<sensor_msgs::PointCloud2>("vertical", 100, 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_); - ROS_INFO("Num of LPR: %d", num_lpr_); - ROS_INFO("Num of min. points: %d", num_min_pts_); - ROS_INFO("Seeds Threshold: %f", th_seeds_); - ROS_INFO("Distance Threshold: %f", th_dist_); - ROS_INFO("Max. range:: %f", max_range_); - ROS_INFO("Min. range:: %f", min_range_); - ROS_INFO("Normal vector threshold: %f", uprightness_thr_); - ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); - - // CZM denotes 'Concentric Zone Model'. Please refer to our paper - 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_); - - if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) { - throw invalid_argument("Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone"); - } - if (elevation_thr_.size() != flatness_thr_.size()) { - throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); - } - - cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % - num_sectors_each_zone_[2] % - num_sectors_each_zone_[3]).str() << endl; - cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % - num_rings_each_zone_[1] % - num_rings_each_zone_[2] % - num_rings_each_zone_[3]).str() << 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; - 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; - num_rings_of_interest_ = elevation_thr_.size(); - - 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"; - poly_list_.polygons.reserve(num_polygons); - - revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - - PlaneViz = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); - pub_revert_pc = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); - pub_reject_pc = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true); - pub_normal = node_handle_.advertise<sensor_msgs::PointCloud2>("normals", 100, true); - pub_noise = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true); - pub_vertical = node_handle_.advertise<sensor_msgs::PointCloud2>("vertical", 100, true); - - min_range_z2_ = (7 * min_range_ + max_range_) / 8.0; - min_range_z3_ = (3 * min_range_ + max_range_) / 4.0; - min_range_z4_ = (min_range_ + max_range_) / 2.0; + ROS_INFO("INITIALIZATION COMPLETE"); + } - min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; - ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), - (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), - (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), - (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), - 2 * M_PI / num_sectors_each_zone_.at(2), - 2 * M_PI / num_sectors_each_zone_.at(3)}; + PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh) { + params_.print_params(); - cout << "INITIALIZATION COMPLETE" << endl; + plane_viz_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); + pub_revert_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); + pub_reject_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true); + pub_normal_ = node_handle_.advertise<sensor_msgs::PointCloud2>("normals", 100, true); + pub_noise_ = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true); + pub_vertical_ = node_handle_.advertise<sensor_msgs::PointCloud2>("vertical", 100, true); - for (int i = 0; i < num_zones_; i++) { - Zone z; - initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); - ConcentricZoneModel_.push_back(z); - } + ROS_INFO("INITIALIZATION COMPLETE"); } void estimate_ground(pcl::PointCloud<PointT> cloud_in, @@ -184,6 +101,10 @@ class PatchWorkpp { std::recursive_mutex mutex_; + ParamsNh params_; + + bool initialized_; + int num_iter_; int num_lpr_; int num_min_pts_; @@ -240,7 +161,7 @@ class PatchWorkpp { jsk_recognition_msgs::PolygonArray poly_list_; - ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical; + ros::Publisher plane_viz_, pub_revert_pc_, pub_reject_pc_, pub_normal_, pub_noise_, pub_vertical_; pcl::PointCloud<PointT> revert_pc_, reject_pc_, noise_pc_, vertical_pc_; pcl::PointCloud<PointT> ground_pc_; @@ -248,6 +169,10 @@ class PatchWorkpp { pcl::PointCloud<PointT> regionwise_ground_, regionwise_nonground_; + void initialize(); + + void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level); + void initialize_zone(Zone &z, int num_sectors, int num_rings); void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings); @@ -335,6 +260,105 @@ void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) { if( verbose_ ) cout << "Flushed patches" << endl; } +template<typename PointT> inline +void PatchWorkpp<PointT>::initialize() { + 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_); + ROS_INFO("Num of LPR: %d", num_lpr_); + ROS_INFO("Num of min. points: %d", num_min_pts_); + ROS_INFO("Seeds Threshold: %f", th_seeds_); + ROS_INFO("Distance Threshold: %f", th_dist_); + ROS_INFO("Max. range:: %f", max_range_); + ROS_INFO("Min. range:: %f", min_range_); + ROS_INFO("Normal vector threshold: %f", uprightness_thr_); + ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); + + // CZM denotes 'Concentric Zone Model'. Please refer to our paper + node_handle_.getParam("czm/num_zones", num_zones_); + node_handle_.getParam("czm/num_sectors_each_zone", num_sectors_each_zone_); + node_handle_.getParam("czm/num_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_); + + if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) { + throw invalid_argument("Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone"); + } + if (elevation_thr_.size() != flatness_thr_.size()) { + throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); + } + + cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % + num_sectors_each_zone_[2] % + num_sectors_each_zone_[3]).str() << endl; + cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % + num_rings_each_zone_[1] % + num_rings_each_zone_[2] % + num_rings_each_zone_[3]).str() << 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; + 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; + num_rings_of_interest_ = elevation_thr_.size(); + + 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"; + poly_list_.polygons.reserve(num_polygons); + + revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + + min_range_z2_ = (7 * min_range_ + max_range_) / 8.0; + min_range_z3_ = (3 * min_range_ + max_range_) / 4.0; + min_range_z4_ = (min_range_ + max_range_) / 2.0; + + min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; + ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), + (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), + (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), + (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; + sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), + 2 * M_PI / num_sectors_each_zone_.at(2), + 2 * M_PI / num_sectors_each_zone_.at(3)}; + + for (int i = 0; i < num_zones_; i++) { + Zone z; + initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); + ConcentricZoneModel_.push_back(z); + } + + initialized_ = true; +} + template<typename PointT> inline void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground) { pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_); @@ -458,6 +482,15 @@ void PatchWorkpp<PointT>::estimate_ground( pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, double &time_taken) { + + if (not initialized_) { + ROS_WARN_STREAM("Waiting for initialization..."); + return; + } + + if (not params_.validate()) { + return; + } unique_lock<recursive_mutex> lock(mutex_); @@ -660,32 +693,32 @@ void PatchWorkpp<PointT>::estimate_ground( pcl::toROSMsg(revert_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_revert_pc.publish(cloud_ROS); + pub_revert_pc_.publish(cloud_ROS); pcl::toROSMsg(reject_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_reject_pc.publish(cloud_ROS); + pub_reject_pc_.publish(cloud_ROS); pcl::toROSMsg(normals_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_normal.publish(cloud_ROS); + pub_normal_.publish(cloud_ROS); pcl::toROSMsg(noise_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_noise.publish(cloud_ROS); + pub_noise_.publish(cloud_ROS); pcl::toROSMsg(vertical_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_vertical.publish(cloud_ROS); + pub_vertical_.publish(cloud_ROS); } if(visualize_) { - PlaneViz.publish(poly_list_); + plane_viz_.publish(poly_list_); } revert_pc_.clear(); diff --git a/launch/demo.launch b/launch/demo.launch index b3b6e42..bc6a1be 100755 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -3,7 +3,7 @@ <arg name="cloud_topic" default="/kitti/velo/pointcloud"/> <node name="ground_segmentation" pkg="patchworkpp" type="demo" output="screen"> - <rosparam command="load" file="$(find patchworkpp)/config/params.yaml" /> + <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" /> <param name="cloud_topic" value="$(arg cloud_topic)"/> </node> diff --git a/config/params.yaml b/params/params.yaml similarity index 93% rename from config/params.yaml rename to params/params.yaml index 9cd6221..b3f494f 100755 --- a/config/params.yaml +++ b/params/params.yaml @@ -2,14 +2,14 @@ save_flag: true # patchworkpp: -sensor_height: 1.723 +sensor_height: 1.2 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_iter: 5 # 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. @@ -24,7 +24,7 @@ adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_select czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] - mum_rings_each_zone: [2, 4, 4, 4] + num_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. diff --git a/src/demo.cpp b/src/demo.cpp index 80020d3..8975caa 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -2,6 +2,7 @@ // For disable PCL complile lib, to use PointXYZIR #define PCL_NO_PRECOMPILE +#include <memory> #include <ros/ros.h> #include <signal.h> #include <sensor_msgs/PointCloud2.h> @@ -12,7 +13,7 @@ using PointType = pcl::PointXYZI; using namespace std; -boost::shared_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg; +std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg; ros::Publisher pub_cloud; ros::Publisher pub_ground; @@ -57,7 +58,7 @@ int main(int argc, char**argv) { pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud"); cout << "Operating patchwork++..." << endl; - PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(&pnh)); + PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh)); pub_cloud = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true); pub_ground = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true); diff --git a/src/demo_reconfigure.cpp b/src/demo_reconfigure.cpp new file mode 100644 index 0000000..502674f --- /dev/null +++ b/src/demo_reconfigure.cpp @@ -0,0 +1,90 @@ +#include <iostream> +// For disable PCL complile lib, to use PointXYZIR +#define PCL_NO_PRECOMPILE + +#include <ros/ros.h> +#include <signal.h> +#include <dynamic_reconfigure/server.h> +#include <sensor_msgs/PointCloud2.h> +#include <pcl_conversions/pcl_conversions.h> +#include <patchworkpp/PatchworkppConfig.h> +#include "patchworkpp/patchworkpp.hpp" + +using PointType = pcl::PointXYZI; +using namespace std; + +std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg; + +ros::Publisher pub_cloud; +ros::Publisher pub_ground; +ros::Publisher pub_non_ground; + +template<typename T> +sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, const ros::Time& stamp, std::string frame_id = "map") { + sensor_msgs::PointCloud2 cloud_ROS; + pcl::toROSMsg(cloud, cloud_ROS); + cloud_ROS.header.stamp = stamp; + cloud_ROS.header.frame_id = frame_id; + return cloud_ROS; +} + +// Callback function for parameter changes +void reconfigureCallback(const patchworkpp::PatchworkppConfig& config, uint32_t level) +{ + // Update your parameters based on the changes + // You can access the parameters using config.param_name + // For example: int parameter = config.int_param; + // Your code to handle parameter changes goes here + ROS_INFO("Updated parameters"); +} + +void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) +{ + double time_taken; + + pcl::PointCloud<PointType> pc_curr; + pcl::PointCloud<PointType> pc_ground; + pcl::PointCloud<PointType> pc_non_ground; + + pcl::fromROSMsg(*cloud_msg, pc_curr); + + PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); + + ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() + << " (running_time: " << time_taken << " sec)" << "\033[0m"); + + pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); +} + +int main(int argc, char**argv) { + + ros::init(argc, argv, "Demo"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + std::string cloud_topic; + pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud"); + + cout << "Operating patchwork++..." << endl; + PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh)); + + pub_cloud = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true); + pub_ground = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true); + pub_non_ground = pnh.advertise<sensor_msgs::PointCloud2>("nonground", 100, true); + + ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud); + + // Create a dynamic reconfigure server + dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server; + dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig>::CallbackType f; + + // Set the callback function + f = boost::bind(&reconfigureCallback, _1, _2); + server.setCallback(f); + + ros::spin(); + + return 0; +} diff --git a/src/offline_kitti.cpp b/src/offline_kitti.cpp index b0d45b2..db12d85 100644 --- a/src/offline_kitti.cpp +++ b/src/offline_kitti.cpp @@ -43,7 +43,7 @@ int main(int argc, char**argv) { ros::Publisher FPPublisher; ros::Publisher FNPublisher; - boost::shared_ptr<PatchWorkpp<PointType> > PatchworkppGroundSeg; + std::unique_ptr<PatchWorkpp<PointType> > PatchworkppGroundSeg; std::string output_csvpath; std::string acc_filename; @@ -80,7 +80,7 @@ int main(int argc, char**argv) { signal(SIGINT, signal_callback_handler); - PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(&nh)); + PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(nh)); data_path = data_path + "/" + seq; KittiLoader loader(data_path); diff --git a/src/video.cpp b/src/video.cpp index aa6360c..e726d21 100644 --- a/src/video.cpp +++ b/src/video.cpp @@ -2,6 +2,7 @@ // For disable PCL complile lib, to use PointXYZIR #define PCL_NO_PRECOMPILE +#include <memory> #include <ros/ros.h> #include <signal.h> #include <sensor_msgs/PointCloud2.h> @@ -45,7 +46,7 @@ int main(int argc, char**argv) { // ros::Publisher FPPublisher; // ros::Publisher FNPublisher; - boost::shared_ptr<PatchWorkpp<PointType> > PatchworkppGroundSeg; + std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg; std::string output_csvpath; std::string acc_filename; @@ -88,7 +89,7 @@ int main(int argc, char**argv) { signal(SIGINT, signal_callback_handler); - PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(&nh)); + PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(nh)); data_path = data_path + "/" + seq; KittiLoader loader(data_path); From 6b1b098c7443994a7fcca605a82fdf37036852bb Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Thu, 25 May 2023 16:55:42 +0200 Subject: [PATCH 2/8] default reconfigure parameters match rosparam servers --- CMakeLists.txt | 6 +- include/patchworkpp/params.hpp | 319 ++++++++++-------- include/patchworkpp/patchworkpp.hpp | 130 +++---- launch/demo.launch | 2 +- launch/demo_reconf.launch | 14 + src/demo.cpp | 2 +- src/{demo_reconfigure.cpp => demo_reconf.cpp} | 5 +- src/offline_kitti.cpp | 6 +- src/video.cpp | 6 +- 9 files changed, 281 insertions(+), 209 deletions(-) create mode 100755 launch/demo_reconf.launch rename src/{demo_reconfigure.cpp => demo_reconf.cpp} (95%) diff --git a/CMakeLists.txt b/CMakeLists.txt index c9a335e..a4c8877 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,9 +74,9 @@ add_executable(demo src/demo.cpp) target_link_libraries(demo ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(demo patchworkpp_generate_messages_cpp) -add_executable(demo_reconfigure src/demo_reconfigure.cpp) -target_link_libraries(demo_reconfigure ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) -add_dependencies(demo_reconfigure patchworkpp_generate_messages_cpp) +add_executable(demo_reconf src/demo_reconf.cpp) +target_link_libraries(demo_reconf ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) +add_dependencies(demo_reconf patchworkpp_generate_messages_cpp) add_executable(video src/video.cpp) target_link_libraries(video ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp index 57231ba..787b903 100644 --- a/include/patchworkpp/params.hpp +++ b/include/patchworkpp/params.hpp @@ -8,118 +8,201 @@ #include <dynamic_reconfigure/server.h> #include <patchworkpp/PatchworkppConfig.h> -class ParamsNh -{ +class ParamsNh { public: - ParamsNh() { - auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2); - server_.setCallback(f); - validate(); - } + ParamsNh() { + auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2); + server_.setCallback(f); + validate(); + } - ParamsNh(const ros::NodeHandle& nh) { - nh.param("verbose", verbose_, false); - nh.param("sensor_height", sensor_height_, 1.723); - nh.param("num_iter", num_iter_, 3); - nh.param("num_lpr", num_lpr_, 20); - nh.param("num_min_pts", num_min_pts_, 10); - nh.param("th_seeds", th_seeds_, 0.4); - nh.param("th_dist", th_dist_, 0.3); - nh.param("th_seeds_v", th_seeds_v_, 0.4); - nh.param("th_dist_v", th_dist_v_, 0.3); - nh.param("max_r", max_range_, 80.0); - nh.param("min_r", min_range_, 2.7); - nh.param("uprightness_thr", uprightness_thr_, 0.5); - nh.param("adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); - nh.param("RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); - nh.param("RNR_intensity_thr", RNR_intensity_thr_, 0.2); - nh.param("max_flatness_storage", max_flatness_storage_, 1000); - nh.param("max_elevation_storage", max_elevation_storage_, 1000); - nh.param("enable_RNR", enable_RNR_, true); - nh.param("enable_RVPF", enable_RVPF_, true); - nh.param("enable_TGR", enable_TGR_, true); - - // CZM denotes 'Concentric Zone Model'. Please refer to our paper - nh.getParam("czm/num_zones", czm.num_zones_); - nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_); - nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_); - nh.getParam("czm/elevation_thresholds", czm.elevation_thresholds_); - nh.getParam("czm/flatness_thresholds", czm.flatness_thresholds_); - - initialized_ = true; - validate(); - } + ParamsNh(const ros::NodeHandle& nh) { + nh.param("verbose", verbose_, false); + nh.param("sensor_height", sensor_height_, 1.723); + nh.param("num_iter", num_iter_, 3); + nh.param("num_lpr", num_lpr_, 20); + nh.param("num_min_pts", num_min_pts_, 10); + nh.param("th_seeds", th_seeds_, 0.4); + nh.param("th_dist", th_dist_, 0.3); + nh.param("th_seeds_v", th_seeds_v_, 0.4); + nh.param("th_dist_v", th_dist_v_, 0.3); + nh.param("max_r", max_range_, 80.0); + nh.param("min_r", min_range_, 2.7); + nh.param("uprightness_thr", uprightness_thr_, 0.5); + nh.param("adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); + nh.param("RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); + nh.param("RNR_intensity_thr", RNR_intensity_thr_, 0.2); + nh.param("max_flatness_storage", max_flatness_storage_, 1000); + nh.param("max_elevation_storage", max_elevation_storage_, 1000); + nh.param("enable_RNR", enable_RNR_, true); + nh.param("enable_RVPF", enable_RVPF_, true); + nh.param("enable_TGR", enable_TGR_, true); + + // CZM denotes 'Concentric Zone Model'. Please refer to our paper + nh.getParam("czm/num_zones", czm.num_zones_); + nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_); + nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_); + nh.getParam("czm/elevation_thresholds", czm.elevation_thr_); + nh.getParam("czm/flatness_thresholds", czm.flatness_thr_); + + validate(); + + num_rings_of_interest_ = czm.elevation_thr_.size(); + + auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; + auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; + auto min_range_z4 = (min_range_ + max_range_) / 2.0; + + min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; + ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), + (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), + (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), + (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; + sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), + 2 * M_PI / czm.num_sectors_each_zone_.at(2), + 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; + + initialized_ = true; + + } - void print_params() const { - ROS_INFO("Sensor Height: %f", sensor_height_); - ROS_INFO("Num of Iteration: %d", num_iter_); - ROS_INFO("Num of LPR: %d", num_lpr_); - ROS_INFO("Num of min. points: %d", num_min_pts_); - ROS_INFO("Seeds Threshold: %f", th_seeds_); - ROS_INFO("Distance Threshold: %f", th_dist_); - ROS_INFO("Max. range:: %f", max_range_); - ROS_INFO("Min. range:: %f", min_range_); - ROS_INFO("Normal vector threshold: %f", uprightness_thr_); - ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); - } + void print_params() const { + ROS_INFO("Sensor Height: %f", sensor_height_); + ROS_INFO("Num of Iteration: %d", num_iter_); + ROS_INFO("Num of LPR: %d", num_lpr_); + ROS_INFO("Num of min. points: %d", num_min_pts_); + ROS_INFO("Seeds Threshold: %f", th_seeds_); + ROS_INFO("Distance Threshold: %f", th_dist_); + ROS_INFO("Max. range:: %f", max_range_); + ROS_INFO("Min. range:: %f", min_range_); + ROS_INFO("Normal vector threshold: %f", uprightness_thr_); + ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); + ROS_INFO("Num. zones: %d", czm.num_zones_); + ROS_INFO_STREAM((boost::format("Num. sectors: %d, %d, %d, %d") % czm.num_sectors_each_zone_[0] % czm.num_sectors_each_zone_[1] % + czm.num_sectors_each_zone_[2] % + czm.num_sectors_each_zone_[3]).str()); + ROS_INFO_STREAM((boost::format("Num. rings: %01d, %01d, %01d, %01d") % czm.num_rings_each_zone_[0] % + czm.num_rings_each_zone_[1] % + czm.num_rings_each_zone_[2] % + czm.num_rings_each_zone_[3]).str()); + ROS_INFO_STREAM((boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % czm.elevation_thr_[0] % czm.elevation_thr_[1] % + czm.elevation_thr_[2] % + czm.elevation_thr_[3]).str()); + ROS_INFO_STREAM((boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % czm.flatness_thr_[0] % czm.flatness_thr_[1] % + czm.flatness_thr_[2] % + czm.flatness_thr_[3]).str()); + } bool validate() { - return check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones and length of num_sectors_each_zone must match") &&\ - check(czm.num_zones_ == czm.num_rings_each_zone_.size(), "num_zones and length of num_rings_each_zone must match") &&\ - check(czm.num_zones_ == czm.elevation_thresholds_.size(), "num_zones and length of elevation_thresholds must match") &&\ - check(czm.num_zones_ == czm.flatness_thresholds_.size(), "num_zones and length of flatness_thresholds must match"); - } + return check(czm.num_zones_ != 4, "Number of zones must be four!") &&\ + check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones and length of num_sectors_each_zone must match") &&\ + check(czm.num_zones_ == czm.num_rings_each_zone_.size(), "num_zones and length of num_rings_each_zone must match") &&\ + check(czm.num_zones_ == czm.elevation_thr_.size(), "num_zones and length of elevation_thresholds must match") &&\ + check(czm.num_zones_ == czm.flatness_thr_.size(), "num_zones and length of flatness_thresholds must match"); + } + + std::string mode_; + bool initialized_; + bool verbose_; + double sensor_height_; + int num_iter_; + int num_lpr_; + int num_min_pts_; + double th_seeds_; + double th_dist_; + double th_seeds_v_; + double th_dist_v_; + double max_range_; + double min_range_; + double uprightness_thr_; + double adaptive_seed_selection_margin_; + double RNR_ver_angle_thr_; + double RNR_intensity_thr_; + int max_flatness_storage_; + int max_elevation_storage_; + bool enable_RNR_; + bool enable_RVPF_; + bool enable_TGR_; + size_t num_rings_of_interest_; + std::vector<double> min_ranges_; + std::vector<double> ring_sizes_; + std::vector<double> sector_sizes_; + + struct CZM + { + int num_zones_; + std::vector<int> num_sectors_each_zone_; + std::vector<int> num_rings_each_zone_; + std::vector<double> elevation_thr_; + std::vector<double> flatness_thr_; + } czm; private: - void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) { - verbose_ = config.verbose; - sensor_height_ = config.sensor_height; - num_iter_ = config.num_iter; - num_lpr_ = config.num_lpr; - num_min_pts_ = config.num_min_pts; - th_seeds_ = config.th_seeds; - th_dist_ = config.th_dist; - th_seeds_v_ = config.th_seeds_v; - th_dist_v_ = config.th_dist_v; - uprightness_thr_ = config.uprightness_thr; - adaptive_seed_selection_margin_ = config.adaptive_seed_selection_margin; - RNR_ver_angle_thr_ = config.RNR_ver_angle_thr; - RNR_intensity_thr_ = config.RNR_intensity_thr; - enable_RNR_ = config.enable_RNR; - enable_RVPF_ = config.enable_RVPF; - enable_TGR_ = config.enable_TGR; - - czm.num_zones_ = config.czm_num_zones; - czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone); - czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone); - czm.elevation_thresholds_ = convert_string_to_vector<double>(config.czm_elevation_thresholds); - czm.flatness_thresholds_ = convert_string_to_vector<double>(config.czm_flatness_thresholds); - - ROS_INFO("Updated params"); - initialized_ = true; - validate(); - } + void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) { + verbose_ = config.verbose; + sensor_height_ = config.sensor_height; + num_iter_ = config.num_iter; + num_lpr_ = config.num_lpr; + num_min_pts_ = config.num_min_pts; + th_seeds_ = config.th_seeds; + th_dist_ = config.th_dist; + th_seeds_v_ = config.th_seeds_v; + th_dist_v_ = config.th_dist_v; + uprightness_thr_ = config.uprightness_thr; + adaptive_seed_selection_margin_ = config.adaptive_seed_selection_margin; + RNR_ver_angle_thr_ = config.RNR_ver_angle_thr; + RNR_intensity_thr_ = config.RNR_intensity_thr; + enable_RNR_ = config.enable_RNR; + enable_RVPF_ = config.enable_RVPF; + enable_TGR_ = config.enable_TGR; + + czm.num_zones_ = config.czm_num_zones; + czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone); + czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone); + czm.elevation_thr_ = convert_string_to_vector<double>(config.czm_elevation_thresholds); + czm.flatness_thr_ = convert_string_to_vector<double>(config.czm_flatness_thresholds); + + num_rings_of_interest_ = czm.elevation_thr_.size(); + + auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; + auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; + auto min_range_z4 = (min_range_ + max_range_) / 2.0; + + validate(); + + min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; + ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), + (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), + (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), + (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; + sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), + 2 * M_PI / czm.num_sectors_each_zone_.at(2), + 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; + + ROS_INFO("Updated params"); + initialized_ = true; + } - template <typename T> - std::vector<T> convert_string_to_vector(const std::string& str, char separator = ',') { - std::vector<T> result; - std::istringstream iss(str); - std::string token; - - while (std::getline(iss, token, separator)) { - token = std::regex_replace(token, std::regex("\\s+"), ""); - T num; - std::stringstream stream(token); - stream >> num; - if (stream.fail()) { - ROS_WARN_STREAM("Can't convert " << token << " to number"); - return result; + template <typename T> + std::vector<T> convert_string_to_vector(const std::string& str, char separator = ',') { + std::vector<T> result; + std::istringstream iss(str); + std::string token; + + while (std::getline(iss, token, separator)) { + token = std::regex_replace(token, std::regex("\\s+"), ""); + T num; + std::stringstream stream(token); + stream >> num; + if (stream.fail()) { + ROS_WARN_STREAM("Can't convert " << token << " to number"); + return result; + } } - } - - return result; - } + + return result; + } bool check(bool assertion, std::string description) { @@ -131,38 +214,6 @@ class ParamsNh return true; } - std::string mode_; - bool initialized_; - bool verbose_; - double sensor_height_; - int num_iter_; - int num_lpr_; - int num_min_pts_; - double th_seeds_; - double th_dist_; - double th_seeds_v_; - double th_dist_v_; - double max_range_; - double min_range_; - double uprightness_thr_; - double adaptive_seed_selection_margin_; - double RNR_ver_angle_thr_; - double RNR_intensity_thr_; - int max_flatness_storage_; - int max_elevation_storage_; - bool enable_RNR_; - bool enable_RVPF_; - bool enable_TGR_; - - struct CZM - { - int num_zones_; - std::vector<int> num_sectors_each_zone_; - std::vector<int> num_rings_each_zone_; - std::vector<double> elevation_thresholds_; - std::vector<double> flatness_thresholds_; - } czm; - dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_; }; diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index e3a9df9..b12dfe6 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -64,8 +64,9 @@ class PatchWorkpp { typedef std::vector<pcl::PointCloud<PointT>> Ring; typedef std::vector<Ring> Zone; - PatchWorkpp() : params_() { + PatchWorkpp() : params_(), using_reconf_(true) { params_.print_params(); + initialize(); plane_viz_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); pub_revert_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); @@ -74,11 +75,17 @@ class PatchWorkpp { pub_noise_ = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true); pub_vertical_ = node_handle_.advertise<sensor_msgs::PointCloud2>("vertical", 100, true); + revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + ROS_INFO("INITIALIZATION COMPLETE"); } - PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh) { + PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh), using_reconf_(false) { params_.print_params(); + initialize(); plane_viz_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); pub_revert_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); @@ -87,6 +94,11 @@ class PatchWorkpp { pub_noise_ = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true); pub_vertical_ = node_handle_.advertise<sensor_msgs::PointCloud2>("vertical", 100, true); + revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + ROS_INFO("INITIALIZATION COMPLETE"); } @@ -120,13 +132,11 @@ class PatchWorkpp { double min_range_; double uprightness_thr_; double adaptive_seed_selection_margin_; - double min_range_z2_; // 12.3625 - double min_range_z3_; // 22.025 - double min_range_z4_; // 41.35 double RNR_ver_angle_thr_; double RNR_intensity_thr_; bool verbose_; + bool using_reconf_; bool enable_RNR_; bool enable_RVPF_; bool enable_TGR_; @@ -174,6 +184,8 @@ class PatchWorkpp { void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level); void initialize_zone(Zone &z, int num_sectors, int num_rings); + void reset_poly_list(); + void reset_concentric_zone_model(); void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings); void flush_patches(std::vector<Zone> &czm); @@ -263,7 +275,6 @@ void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) { template<typename PointT> inline void PatchWorkpp<PointT>::initialize() { 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); @@ -284,6 +295,32 @@ void PatchWorkpp<PointT>::initialize() { node_handle_.param("enable_RVPF", enable_RVPF_, true); node_handle_.param("enable_TGR", enable_TGR_, true); + // CZM denotes 'Concentric Zone Model'. Please refer to our paper + node_handle_.getParam("czm/num_zones", num_zones_); + node_handle_.getParam("czm/num_sectors_each_zone", num_sectors_each_zone_); + node_handle_.getParam("czm/num_rings_each_zone", num_rings_each_zone_); + node_handle_.getParam("czm/elevation_thresholds", elevation_thr_); + node_handle_.getParam("czm/flatness_thresholds", flatness_thr_); + node_handle_.param("visualize", visualize_, true); + + num_rings_of_interest_ = elevation_thr_.size(); + + auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; + auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; + auto min_range_z4 = (min_range_ + max_range_) / 2.0; + + min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; + ring_sizes_ = {(min_range_z2 - min_range_) / num_rings_each_zone_.at(0), + (min_range_z3 - min_range_z2) / num_rings_each_zone_.at(1), + (min_range_z4 - min_range_z3) / num_rings_each_zone_.at(2), + (max_range_ - min_range_z4) / num_rings_each_zone_.at(3)}; + sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), + 2 * M_PI / num_sectors_each_zone_.at(2), + 2 * M_PI / num_sectors_each_zone_.at(3)}; + + reset_poly_list(); + reset_concentric_zone_model(); + ROS_INFO("Sensor Height: %f", sensor_height_); ROS_INFO("Num of Iteration: %d", num_iter_); ROS_INFO("Num of LPR: %d", num_lpr_); @@ -294,69 +331,40 @@ void PatchWorkpp<PointT>::initialize() { ROS_INFO("Min. range:: %f", min_range_); ROS_INFO("Normal vector threshold: %f", uprightness_thr_); ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); - - // CZM denotes 'Concentric Zone Model'. Please refer to our paper - node_handle_.getParam("czm/num_zones", num_zones_); - node_handle_.getParam("czm/num_sectors_each_zone", num_sectors_each_zone_); - node_handle_.getParam("czm/num_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_); - - if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) { - throw invalid_argument("Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone"); - } - if (elevation_thr_.size() != flatness_thr_.size()) { - throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); - } - - cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % + ROS_INFO_STREAM((boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % num_sectors_each_zone_[2] % - num_sectors_each_zone_[3]).str() << endl; - cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % + num_sectors_each_zone_[3]).str()); + ROS_INFO_STREAM((boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % num_rings_each_zone_[1] % num_rings_each_zone_[2] % - num_rings_each_zone_[3]).str() << endl; - cout << (boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % elevation_thr_[0] % elevation_thr_[1] % + num_rings_each_zone_[3]).str()); + ROS_INFO_STREAM((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; - cout << (boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % flatness_thr_[0] % flatness_thr_[1] % + elevation_thr_[3]).str()); + ROS_INFO_STREAM((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; - num_rings_of_interest_ = elevation_thr_.size(); - - 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"; - poly_list_.polygons.reserve(num_polygons); - - revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - - min_range_z2_ = (7 * min_range_ + max_range_) / 8.0; - min_range_z3_ = (3 * min_range_ + max_range_) / 4.0; - min_range_z4_ = (min_range_ + max_range_) / 2.0; + flatness_thr_[3]).str()); - min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; - ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), - (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), - (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), - (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), - 2 * M_PI / num_sectors_each_zone_.at(2), - 2 * M_PI / num_sectors_each_zone_.at(3)}; + initialized_ = true; +} - for (int i = 0; i < num_zones_; i++) { - Zone z; - initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); - ConcentricZoneModel_.push_back(z); - } +template<typename PointT> inline +void PatchWorkpp<PointT>::reset_poly_list() { + 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"; + poly_list_.polygons.clear(); + poly_list_.polygons.reserve(num_polygons); +} - initialized_ = true; +template<typename PointT> inline +void PatchWorkpp<PointT>::reset_concentric_zone_model() { + ConcentricZoneModel_.clear(); + for (int i = 0; i < num_zones_; i++) { + Zone z; + initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); + ConcentricZoneModel_.push_back(z); + } } template<typename PointT> inline @@ -488,7 +496,7 @@ void PatchWorkpp<PointT>::estimate_ground( return; } - if (not params_.validate()) { + if (using_reconf_ && not params_.validate()) { return; } diff --git a/launch/demo.launch b/launch/demo.launch index bc6a1be..38d2129 100755 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -1,6 +1,6 @@ <launch> -<arg name="cloud_topic" default="/kitti/velo/pointcloud"/> +<arg name="cloud_topic" default="/velodyne_points"/> <node name="ground_segmentation" pkg="patchworkpp" type="demo" output="screen"> <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" /> diff --git a/launch/demo_reconf.launch b/launch/demo_reconf.launch new file mode 100755 index 0000000..3cf4554 --- /dev/null +++ b/launch/demo_reconf.launch @@ -0,0 +1,14 @@ +<launch> + + <arg name="cloud_topic" default="/velodyne_points"/> + + <node name="rqt_conf_gui" pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen"/> + + <node name="ground_segmentation" pkg="patchworkpp" type="demo_reconf" output="screen"> + <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" /> + <param name="cloud_topic" value="$(arg cloud_topic)"/> + </node> + + <node name="$(anon rviz)" pkg="rviz" type="rviz" args="-d $(find patchworkpp)/rviz/demo.rviz"/> + +</launch> diff --git a/src/demo.cpp b/src/demo.cpp index 8975caa..7da6389 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -57,7 +57,7 @@ int main(int argc, char**argv) { std::string cloud_topic; pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud"); - cout << "Operating patchwork++..." << endl; + ROS_INFO("Operating patchwork++..."); PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh)); pub_cloud = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true); diff --git a/src/demo_reconfigure.cpp b/src/demo_reconf.cpp similarity index 95% rename from src/demo_reconfigure.cpp rename to src/demo_reconf.cpp index 502674f..c90b40c 100644 --- a/src/demo_reconfigure.cpp +++ b/src/demo_reconf.cpp @@ -11,7 +11,6 @@ #include "patchworkpp/patchworkpp.hpp" using PointType = pcl::PointXYZI; -using namespace std; std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg; @@ -67,8 +66,8 @@ int main(int argc, char**argv) { std::string cloud_topic; pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud"); - cout << "Operating patchwork++..." << endl; - PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh)); + ROS_INFO("Operating patchwork++..."); + PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>()); pub_cloud = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true); pub_ground = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true); diff --git a/src/offline_kitti.cpp b/src/offline_kitti.cpp index db12d85..ee8788d 100644 --- a/src/offline_kitti.cpp +++ b/src/offline_kitti.cpp @@ -15,7 +15,7 @@ using PointType = PointXYZILID; using namespace std; void signal_callback_handler(int signum) { - cout << "Caught Ctrl + c " << endl; + ROS_INFO("Caught Ctrl + c "); // Terminate program exit(signum); } @@ -87,14 +87,14 @@ int main(int argc, char**argv) { int N = loader.size(); for (int n = init_idx; n < N; ++n) { - cout << n << "th node come" << endl; + ROS_INFO("th node come"); pcl::PointCloud<PointType> pc_curr; loader.get_cloud(n, pc_curr); pcl::PointCloud<PointType> pc_ground; pcl::PointCloud<PointType> pc_non_ground; static double time_taken; - cout << "Operating patchwork++..." << endl; + ROS_INFO("Operating patchwork++..."); PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); diff --git a/src/video.cpp b/src/video.cpp index e726d21..0dd7ca0 100644 --- a/src/video.cpp +++ b/src/video.cpp @@ -16,7 +16,7 @@ using PointType = PointXYZILID; using namespace std; void signal_callback_handler(int signum) { - cout << "Caught Ctrl + c " << endl; + ROS_INFO("Caught Ctrl + c "); // Terminate program exit(signum); } @@ -96,14 +96,14 @@ int main(int argc, char**argv) { int N = loader.size(); for (int n = init_idx; n < N; ++n) { - cout << n << "th node come" << endl; + ROS_INFO("th node come"); pcl::PointCloud<PointType> pc_curr; loader.get_cloud(n, pc_curr); pcl::PointCloud<PointType> pc_ground; pcl::PointCloud<PointType> pc_non_ground; static double time_taken; - cout << "Operating patchwork++..." << endl; + ROS_INFO("Operating patchwork++..."); PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); From 38e00a637925a27d34e6ed1e184b09383b99128a Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Thu, 25 May 2023 17:39:59 +0200 Subject: [PATCH 3/8] move to ParamsHandler for regular demo --- cfg/patchwork.cfg | 2 +- include/patchworkpp/params.hpp | 31 +-- include/patchworkpp/patchworkpp.hpp | 317 +++++++++------------------- src/demo.cpp | 14 +- src/demo_reconf.cpp | 16 +- 5 files changed, 139 insertions(+), 241 deletions(-) diff --git a/cfg/patchwork.cfg b/cfg/patchwork.cfg index f5fb8da..28b75eb 100644 --- a/cfg/patchwork.cfg +++ b/cfg/patchwork.cfg @@ -12,7 +12,7 @@ gen.add("verbose", bool_t, 0, "Verbose mode for debugging", False) gen.add("visualize", bool_t, 0, "Flag to visualize the ground likelihood estimation", True) # Ground Plane Fitting parameters -gen.add("num_iter", int_t, 0, "Number of iterations for ground plane estimation using PCA", 3, 0, 10) +gen.add("num_iter", int_t, 0, "Number of iterations for ground plane estimation using PCA", 3, 1, 10) gen.add("num_lpr", int_t, 0, "Maximum number of points to be selected as lowest points representative", 20, 0, 100) gen.add("num_min_pts", int_t, 0, "Minimum number of points to be estimated as ground plane in each patch", 10, 0, 100) gen.add("th_seeds", double_t, 0, "Threshold for lowest point representatives using in initial seeds selection of ground points", 0.3, 0.0, 1.0) diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp index 787b903..6fb8fb2 100644 --- a/include/patchworkpp/params.hpp +++ b/include/patchworkpp/params.hpp @@ -2,22 +2,25 @@ #define PATCHWORKPP_PARAMS #include <regex> +#include <mutex> #include <vector> #include <stdexcept> #include <ros/ros.h> #include <dynamic_reconfigure/server.h> #include <patchworkpp/PatchworkppConfig.h> -class ParamsNh { +class ParamsHandler { public: - ParamsNh() { - auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2); + ParamsHandler(std::recursive_mutex& mutex) : mutex_(mutex) { + auto f = boost::bind(&ParamsHandler::reconfigure_callback, this, _1, _2); server_.setCallback(f); + std::unique_lock<std::recursive_mutex> lock(mutex_); validate(); } - ParamsNh(const ros::NodeHandle& nh) { + ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex) { nh.param("verbose", verbose_, false); + nh.param("verbose", visualize_, true); nh.param("sensor_height", sensor_height_, 1.723); nh.param("num_iter", num_iter_, 3); nh.param("num_lpr", num_lpr_, 20); @@ -104,6 +107,7 @@ class ParamsNh { std::string mode_; bool initialized_; bool verbose_; + bool visualize_; double sensor_height_; int num_iter_; int num_lpr_; @@ -139,7 +143,9 @@ class ParamsNh { private: void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) { + std::unique_lock<std::recursive_mutex> lock(mutex_); verbose_ = config.verbose; + visualize_ = config.visualize; sensor_height_ = config.sensor_height; num_iter_ = config.num_iter; num_lpr_ = config.num_lpr; @@ -205,16 +211,17 @@ class ParamsNh { } - bool check(bool assertion, std::string description) { - if (not assertion) { - ROS_WARN_STREAM(description); - return false; - } + bool check(bool assertion, std::string description) { + if (not assertion) { + ROS_WARN_STREAM(description); + return false; + } - return true; - } + return true; + } - dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_; + std::recursive_mutex& mutex_; + dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_; }; #endif \ No newline at end of file diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index b12dfe6..eabc4a7 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -64,9 +64,8 @@ class PatchWorkpp { typedef std::vector<pcl::PointCloud<PointT>> Ring; typedef std::vector<Ring> Zone; - PatchWorkpp() : params_(), using_reconf_(true) { + PatchWorkpp() : mutex_(), params_(mutex_), using_reconf_(true) { params_.print_params(); - initialize(); plane_viz_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); pub_revert_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); @@ -80,12 +79,14 @@ class PatchWorkpp { regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + reset_poly_list(); + reset_concentric_zone_model(); + ROS_INFO("INITIALIZATION COMPLETE"); } - PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh), using_reconf_(false) { + PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), mutex_(), params_(nh, mutex_), using_reconf_(false) { params_.print_params(); - initialize(); plane_viz_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); pub_revert_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); @@ -99,10 +100,13 @@ class PatchWorkpp { regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + reset_poly_list(); + reset_concentric_zone_model(); + ROS_INFO("INITIALIZATION COMPLETE"); } - void estimate_ground(pcl::PointCloud<PointT> cloud_in, + bool estimate_ground(pcl::PointCloud<PointT> cloud_in, pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, double &time_taken); private: @@ -113,35 +117,10 @@ class PatchWorkpp { std::recursive_mutex mutex_; - ParamsNh params_; - - bool initialized_; - - int num_iter_; - int num_lpr_; - int num_min_pts_; - int num_zones_; - int num_rings_of_interest_; - - double sensor_height_; - double th_seeds_; - double th_dist_; - double th_seeds_v_; - double th_dist_v_; - double max_range_; - double min_range_; - double uprightness_thr_; - double adaptive_seed_selection_margin_; - double RNR_ver_angle_thr_; - double RNR_intensity_thr_; - - bool verbose_; + ParamsHandler params_; + bool using_reconf_; - bool enable_RNR_; - bool enable_RVPF_; - bool enable_TGR_; - int max_flatness_storage_, max_elevation_storage_; std::vector<double> update_flatness_[4]; std::vector<double> update_elevation_[4]; @@ -153,18 +132,6 @@ class PatchWorkpp { Eigen::Matrix3f cov_; Eigen::Vector4f pc_mean_; - // For visualization - bool visualize_; - - vector<int> num_sectors_each_zone_; - vector<int> num_rings_each_zone_; - - vector<double> sector_sizes_; - vector<double> ring_sizes_; - vector<double> min_ranges_; - vector<double> elevation_thr_; - vector<double> flatness_thr_; - queue<int> noise_idxs_; vector<Zone> ConcentricZoneModel_; @@ -179,8 +146,6 @@ class PatchWorkpp { pcl::PointCloud<PointT> regionwise_ground_, regionwise_nonground_; - void initialize(); - void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level); void initialize_zone(Zone &z, int num_sectors, int num_rings); @@ -261,97 +226,20 @@ void PatchWorkpp<PointT>::flush_patches_in_zone(Zone &patches, int num_sectors, template<typename PointT> inline void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) { - for (int k = 0; k < num_zones_; k++) { - for (int i = 0; i < num_rings_each_zone_[k]; i++) { - for (int j = 0; j < num_sectors_each_zone_[k]; j++) { + for (int k = 0; k < params_.czm.num_zones_; k++) { + for (int i = 0; i < params_.czm.num_rings_each_zone_[k]; i++) { + for (int j = 0; j < params_.czm.num_sectors_each_zone_[k]; j++) { if (!czm[k][i][j].points.empty()) czm[k][i][j].points.clear(); } } } - if( verbose_ ) cout << "Flushed patches" << endl; -} - -template<typename PointT> inline -void PatchWorkpp<PointT>::initialize() { - 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); - - // CZM denotes 'Concentric Zone Model'. Please refer to our paper - node_handle_.getParam("czm/num_zones", num_zones_); - node_handle_.getParam("czm/num_sectors_each_zone", num_sectors_each_zone_); - node_handle_.getParam("czm/num_rings_each_zone", num_rings_each_zone_); - node_handle_.getParam("czm/elevation_thresholds", elevation_thr_); - node_handle_.getParam("czm/flatness_thresholds", flatness_thr_); - node_handle_.param("visualize", visualize_, true); - - num_rings_of_interest_ = elevation_thr_.size(); - - auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; - auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; - auto min_range_z4 = (min_range_ + max_range_) / 2.0; - - min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; - ring_sizes_ = {(min_range_z2 - min_range_) / num_rings_each_zone_.at(0), - (min_range_z3 - min_range_z2) / num_rings_each_zone_.at(1), - (min_range_z4 - min_range_z3) / num_rings_each_zone_.at(2), - (max_range_ - min_range_z4) / num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), - 2 * M_PI / num_sectors_each_zone_.at(2), - 2 * M_PI / num_sectors_each_zone_.at(3)}; - - reset_poly_list(); - reset_concentric_zone_model(); - - ROS_INFO("Sensor Height: %f", sensor_height_); - ROS_INFO("Num of Iteration: %d", num_iter_); - ROS_INFO("Num of LPR: %d", num_lpr_); - ROS_INFO("Num of min. points: %d", num_min_pts_); - ROS_INFO("Seeds Threshold: %f", th_seeds_); - ROS_INFO("Distance Threshold: %f", th_dist_); - ROS_INFO("Max. range:: %f", max_range_); - ROS_INFO("Min. range:: %f", min_range_); - ROS_INFO("Normal vector threshold: %f", uprightness_thr_); - ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); - ROS_INFO("Num. zones: %d", num_zones_); - ROS_INFO_STREAM((boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % - num_sectors_each_zone_[2] % - num_sectors_each_zone_[3]).str()); - ROS_INFO_STREAM((boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % - num_rings_each_zone_[1] % - num_rings_each_zone_[2] % - num_rings_each_zone_[3]).str()); - ROS_INFO_STREAM((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()); - ROS_INFO_STREAM((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()); - - initialized_ = true; + if( params_ .verbose_ ) cout << "Flushed patches" << endl; } template<typename PointT> inline void PatchWorkpp<PointT>::reset_poly_list() { - int num_polygons = std::inner_product(num_rings_each_zone_.begin(), num_rings_each_zone_.end(), num_sectors_each_zone_.begin(), 0); + int num_polygons = std::inner_product(params_.czm.num_rings_each_zone_.begin(), params_.czm.num_rings_each_zone_.end(), params_.czm.num_sectors_each_zone_.begin(), 0); poly_list_.header.frame_id = "map"; poly_list_.polygons.clear(); poly_list_.polygons.reserve(num_polygons); @@ -360,9 +248,9 @@ void PatchWorkpp<PointT>::reset_poly_list() { template<typename PointT> inline void PatchWorkpp<PointT>::reset_concentric_zone_model() { ConcentricZoneModel_.clear(); - for (int i = 0; i < num_zones_; i++) { + for (int i = 0; i < params_.czm.num_zones_; i++) { Zone z; - initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); + initialize_zone(z, params_.czm.num_sectors_each_zone_[i], params_.czm.num_rings_each_zone_[i]); ConcentricZoneModel_.push_back(z); } } @@ -399,7 +287,7 @@ void PatchWorkpp<PointT>::extract_initial_seeds( int init_idx = 0; if (zone_idx == 0) { for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { + if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) { ++init_idx; } else { break; @@ -408,13 +296,13 @@ void PatchWorkpp<PointT>::extract_initial_seeds( } // Calculate the mean height value. - for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { + for (int i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 - // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ + // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_ for (int i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lpr_height + th_seed) { init_seeds.points.push_back(p_sorted.points[i]); @@ -435,7 +323,7 @@ void PatchWorkpp<PointT>::extract_initial_seeds( int init_idx = 0; if (zone_idx == 0) { for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { + if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) { ++init_idx; } else { break; @@ -444,15 +332,15 @@ void PatchWorkpp<PointT>::extract_initial_seeds( } // Calculate the mean height value. - for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { + for (int i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 - // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ + // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_ for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < lpr_height + th_seeds_) { + if (p_sorted.points[i].z < lpr_height + params_.th_seeds_) { init_seeds.points.push_back(p_sorted.points[i]); } } @@ -467,7 +355,7 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud double z = cloud_in[i].z; double ver_angle_in_deg = atan2(z, r)*180/M_PI; - if ( ver_angle_in_deg < RNR_ver_angle_thr_ && z < -sensor_height_-0.8 && cloud_in[i].intensity < RNR_intensity_thr_) + if ( ver_angle_in_deg < params_.RNR_ver_angle_thr_ && z < -params_.sensor_height_-0.8 && cloud_in[i].intensity < params_.RNR_intensity_thr_) { cloud_nonground.push_back(cloud_in[i]); noise_pc_.push_back(cloud_in[i]); @@ -475,7 +363,7 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud } } - if (verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl; + if (params_ .verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl; } /* @@ -485,23 +373,24 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud */ template<typename PointT> inline -void PatchWorkpp<PointT>::estimate_ground( +bool PatchWorkpp<PointT>::estimate_ground( pcl::PointCloud<PointT> cloud_in, pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, double &time_taken) { - if (not initialized_) { + unique_lock<recursive_mutex> lock(mutex_); + + if (not params_.initialized_ || params_.num_iter_ < 1) { ROS_WARN_STREAM("Waiting for initialization..."); - return; + return false; } if (using_reconf_ && not params_.validate()) { - return; + ROS_WARN_STREAM("Invalid parameters"); + return false; } - unique_lock<recursive_mutex> 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(); @@ -520,7 +409,7 @@ void PatchWorkpp<PointT>::estimate_ground( cloud_nonground.clear(); // 1. Reflected Noise Removal (RNR) - if (enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground); + if (params_.enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground); t1 = ros::Time::now().toSec(); @@ -537,14 +426,14 @@ void PatchWorkpp<PointT>::estimate_ground( std::vector<RevertCandidate<PointT>> candidates; std::vector<double> ringwise_flatness; - for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) { + for (int zone_idx = 0; zone_idx < params_.czm.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) { + for (int ring_idx = 0; ring_idx < params_.czm.num_rings_each_zone_[zone_idx]; ++ring_idx) { + for (int sector_idx = 0; sector_idx < params_.czm.num_sectors_each_zone_[zone_idx]; ++sector_idx) { - if (zone[ring_idx][sector_idx].points.size() < num_min_pts_) + if (zone[ring_idx][sector_idx].points.size() < params_.num_min_pts_) { cloud_nonground += zone[ring_idx][sector_idx]; continue; @@ -576,7 +465,7 @@ void PatchWorkpp<PointT>::estimate_ground( double heading = 0.0; for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i); - if (visualize_) { + if (params_.visualize_) { auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3); polygons.header = poly_list_.header; poly_list_.polygons.push_back(polygons); @@ -604,10 +493,10 @@ void PatchWorkpp<PointT>::estimate_ground( Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition ) */ - bool is_upright = ground_uprightness > uprightness_thr_; - bool is_not_elevated = ground_elevation < elevation_thr_[concentric_idx]; - bool is_flat = ground_flatness < flatness_thr_[concentric_idx]; - bool is_near_zone = concentric_idx < num_rings_of_interest_; + bool is_upright = ground_uprightness > params_.uprightness_thr_; + bool is_not_elevated = ground_elevation < params_.czm.elevation_thr_[concentric_idx]; + bool is_flat = ground_flatness < params_.czm.flatness_thr_[concentric_idx]; + bool is_near_zone = concentric_idx < params_.num_rings_of_interest_; bool is_heading_outside = heading < 0.0; /* @@ -656,7 +545,7 @@ void PatchWorkpp<PointT>::estimate_ground( if (!candidates.empty()) { - if (enable_TGR_) + if (params_.enable_TGR_) { temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx); } @@ -695,7 +584,7 @@ void PatchWorkpp<PointT>::estimate_ground( // cout << "Time taken to Revert: " << t_revert << endl; // cout << "Time taken to update : " << end - t_update << endl; - if (visualize_) + if (params_.visualize_) { sensor_msgs::PointCloud2 cloud_ROS; pcl::toROSMsg(revert_pc_, cloud_ROS); @@ -724,7 +613,7 @@ void PatchWorkpp<PointT>::estimate_ground( pub_vertical_.publish(cloud_ROS); } - if(visualize_) + if(params_.visualize_) { plane_viz_.publish(poly_list_); } @@ -734,34 +623,36 @@ void PatchWorkpp<PointT>::estimate_ground( normals_.clear(); noise_pc_.clear(); vertical_pc_.clear(); + + return true; } template<typename PointT> inline void PatchWorkpp<PointT>::update_elevation_thr(void) { - for (int i=0; i<num_rings_of_interest_; i++) + for (int i=0; i<params_.num_rings_of_interest_; i++) { if (update_elevation_[i].empty()) continue; double update_mean = 0.0, update_stdev = 0.0; calc_mean_stdev(update_elevation_[i], update_mean, update_stdev); if (i==0) { - elevation_thr_[i] = update_mean + 3*update_stdev; - sensor_height_ = -update_mean; + params_.czm.elevation_thr_[i] = update_mean + 3*update_stdev; + params_.sensor_height_ = -update_mean; } - else elevation_thr_[i] = update_mean + 2*update_stdev; + else params_.czm.elevation_thr_[i] = update_mean + 2*update_stdev; - // if (verbose_) cout << "elevation threshold [" << i << "]: " << elevation_thr_[i] << endl; + // if (params_ .verbose_) cout << "elevation threshold [" << i << "]: " << params_.czm.elevation_thr_[i] << endl; - int exceed_num = update_elevation_[i].size() - max_elevation_storage_; + int exceed_num = update_elevation_[i].size() - params_.max_elevation_storage_; if (exceed_num > 0) update_elevation_[i].erase(update_elevation_[i].begin(), update_elevation_[i].begin() + exceed_num); } - if (verbose_) + if (params_ .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; + cout << "sensor height: " << params_.sensor_height_ << endl; + cout << (boost::format("params_.czm.elevation_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") + % params_.czm.elevation_thr_[0] % params_.czm.elevation_thr_[1] % params_.czm.elevation_thr_[2] % params_.czm.elevation_thr_[3]).str() << endl; } return; @@ -770,25 +661,25 @@ void PatchWorkpp<PointT>::update_elevation_thr(void) template<typename PointT> inline void PatchWorkpp<PointT>::update_flatness_thr(void) { - for (int i=0; i<num_rings_of_interest_; i++) + for (int i=0; i<params_.num_rings_of_interest_; i++) { if (update_flatness_[i].empty()) break; if (update_flatness_[i].size() <= 1) break; double update_mean = 0.0, update_stdev = 0.0; calc_mean_stdev(update_flatness_[i], update_mean, update_stdev); - flatness_thr_[i] = update_mean+update_stdev; + params_.czm.flatness_thr_[i] = update_mean+update_stdev; - // if (verbose_) { cout << "flatness threshold [" << i << "]: " << flatness_thr_[i] << endl; } + // if (params_ .verbose_) { cout << "flatness threshold [" << i << "]: " << params_.czm.flatness_thr_[i] << endl; } - int exceed_num = update_flatness_[i].size() - max_flatness_storage_; + int exceed_num = update_flatness_[i].size() - params_.max_flatness_storage_; if (exceed_num > 0) update_flatness_[i].erase(update_flatness_[i].begin(), update_flatness_[i].begin() + exceed_num); } - if (verbose_) + if (params_ .verbose_) { - 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; + cout << (boost::format("params_.czm.flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") + % params_.czm.flatness_thr_[0] % params_.czm.flatness_thr_[1] % params_.czm.flatness_thr_[2] % params_.czm.flatness_thr_[3]).str() << endl; } return; @@ -799,12 +690,12 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ std::vector<double> ring_flatness, std::vector<RevertCandidate<PointT>> candidates, int concentric_idx) { - if (verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl; + if (params_ .verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl; double mean_flatness = 0.0, stdev_flatness = 0.0; calc_mean_stdev(ring_flatness, mean_flatness, stdev_flatness); - if (verbose_) + if (params_ .verbose_) { cout << "[" << candidates[0].concentric_idx << ", " << candidates[0].sector_idx << "]" << " mean_flatness: " << mean_flatness << ", stdev_flatness: " << stdev_flatness << std::endl; @@ -815,7 +706,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ RevertCandidate<PointT> candidate = candidates[i]; // Debug - if(verbose_) + if(params_ .verbose_) { cout << "\033[1;33m" << candidate.sector_idx << "th flat_sector_candidate" << " / flatness: " << candidate.ground_flatness @@ -827,22 +718,22 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ double mu_flatness = mean_flatness + 1.5*stdev_flatness; double prob_flatness = 1/(1+exp( (candidate.ground_flatness-mu_flatness)/(mu_flatness/10) )); - if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < th_dist_*th_dist_) prob_flatness = 1.0; + if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < params_.th_dist_* params_.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 > params_.czm.elevation_thr_[concentric_idx]) { - // if (verbose_) cout << "line_dir: " << candidate.line_dir << endl; + // if (params_ .verbose_) cout << "line_dir: " << candidate.line_dir << endl; prob_line = 0.0; } bool revert = prob_line*prob_flatness > 0.5; - if ( concentric_idx < num_rings_of_interest_ ) + if ( concentric_idx < params_.num_rings_of_interest_ ) { if (revert) { - if (verbose_) + if (params_ .verbose_) { cout << "\033[1;32m" << "REVERT TRUE" << "\033[0m" << endl; } @@ -852,7 +743,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ } else { - if (verbose_) + if (params_ .verbose_) { cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl; } @@ -862,7 +753,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ } } - if (verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl; + if (params_ .verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl; } // For adaptive @@ -882,14 +773,14 @@ void PatchWorkpp<PointT>::extract_piecewiseground( pcl::PointCloud<PointT> src_wo_verticals; src_wo_verticals = src; - if (enable_RVPF_) + if (params_.enable_RVPF_) { - for (int i = 0; i < num_iter_; i++) + for (int i = 0; i < params_.num_iter_; i++) { - extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_); + extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, params_.th_seeds_v_); estimate_plane(ground_pc_); - if (zone_idx == 0 && normal_(2) < uprightness_thr_) + if (zone_idx == 0 && normal_(2) < params_.uprightness_thr_) { pcl::PointCloud<PointT> src_tmp; src_tmp = src_wo_verticals; @@ -904,7 +795,7 @@ void PatchWorkpp<PointT>::extract_piecewiseground( Eigen::VectorXf result = points * normal_; for (int r = 0; r < result.rows(); r++) { - if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) { + if (result[r] < params_.th_dist_v_ - d_ && result[r] > -params_.th_dist_v_ - d_) { non_ground_dst.points.push_back(src_tmp[r]); vertical_pc_.points.push_back(src_tmp[r]); } else { @@ -929,7 +820,7 @@ void PatchWorkpp<PointT>::extract_piecewiseground( points.row(j++) << p.x, p.y, p.z; } - for (int i = 0; i < num_iter_; i++) { + for (int i = 0; i < params_.num_iter_; i++) { ground_pc_.clear(); @@ -937,12 +828,12 @@ void PatchWorkpp<PointT>::extract_piecewiseground( Eigen::VectorXf result = points * normal_; // threshold filter for (int r = 0; r < result.rows(); r++) { - if (i < num_iter_ - 1) { - if (result[r] < th_dist_ - d_ ) { + if (i < params_.num_iter_ - 1) { + if (result[r] < params_.th_dist_ - d_ ) { ground_pc_.points.push_back(src_wo_verticals[r]); } } else { // Final stage - if (result[r] < th_dist_ - d_ ) { + if (result[r] < params_.th_dist_ - d_ ) { dst.points.push_back(src_wo_verticals[r]); } else { non_ground_dst.points.push_back(src_wo_verticals[r]); @@ -950,7 +841,7 @@ void PatchWorkpp<PointT>::extract_piecewiseground( } } - if (i < num_iter_ -1) estimate_plane(ground_pc_); + if (i < params_.num_iter_ -1) estimate_plane(ground_pc_); else estimate_plane(dst); } } @@ -962,16 +853,16 @@ geometry_msgs::PolygonStamped PatchWorkpp<PointT>::set_polygons(int zone_idx, in geometry_msgs::Point32 point; // RL - double zone_min_range = min_ranges_[zone_idx]; - double r_len = r_idx * ring_sizes_[zone_idx] + zone_min_range; - double angle = theta_idx * sector_sizes_[zone_idx]; + double zone_min_range = params_.min_ranges_[zone_idx]; + double r_len = r_idx * params_.ring_sizes_[zone_idx] + zone_min_range; + double angle = theta_idx * params_.sector_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); // RU - r_len = r_len + ring_sizes_[zone_idx]; + r_len = r_len + params_.ring_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; @@ -979,21 +870,21 @@ geometry_msgs::PolygonStamped PatchWorkpp<PointT>::set_polygons(int zone_idx, in // RU -> LU for (int idx = 1; idx <= num_split; ++idx) { - angle = angle + sector_sizes_[zone_idx] / num_split; + angle = angle + params_.sector_sizes_[zone_idx] / num_split; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); } - r_len = r_len - ring_sizes_[zone_idx]; + r_len = r_len - params_.ring_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); for (int idx = 1; idx < num_split; ++idx) { - angle = angle - sector_sizes_[zone_idx] / num_split; + angle = angle - params_.sector_sizes_[zone_idx] / num_split; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; @@ -1010,10 +901,10 @@ void PatchWorkpp<PointT>::set_ground_likelihood_estimation_status( const double z_vec, const double z_elevation, const double ground_flatness) { - if (z_vec > uprightness_thr_) { //orthogonal - if (concentric_idx < num_rings_of_interest_) { - if (z_elevation > elevation_thr_[concentric_idx]) { - if (flatness_thr_[concentric_idx] > ground_flatness) { + if (z_vec > params_.uprightness_thr_) { //orthogonal + if (concentric_idx < params_.num_rings_of_interest_) { + if (z_elevation > params_.czm.elevation_thr_[concentric_idx]) { + if (params_.czm.flatness_thr_[concentric_idx] > ground_flatness) { poly_list_.likelihood.push_back(FLAT_ENOUGH); } else { poly_list_.likelihood.push_back(TOO_HIGH_ELEVATION); @@ -1070,17 +961,17 @@ void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector PointT pt = src.points[i]; double r = xy2radius(pt.x, pt.y); - if ((r <= max_range_) && (r > min_range_)) { + if ((r <= params_.max_range_) && (r > params_.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; + if ( r < params_.min_ranges_[1] ) zone_idx = 0; + else if ( r < params_.min_ranges_[2] ) zone_idx = 1; + else if ( r < params_.min_ranges_[3] ) zone_idx = 2; else zone_idx = 3; - int ring_idx = min(static_cast<int>(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1); - int sector_idx = min(static_cast<int>((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1); + int ring_idx = min(static_cast<int>(((r - params_.min_ranges_[zone_idx]) / params_.ring_sizes_[zone_idx])), params_.czm.num_rings_each_zone_[zone_idx] - 1); + int sector_idx = min(static_cast<int>((theta / params_.sector_sizes_[zone_idx])), params_.czm.num_sectors_each_zone_[zone_idx] - 1); czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt); } @@ -1089,7 +980,7 @@ void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector } } - if (verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl; + if (params_ .verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl; } #endif diff --git a/src/demo.cpp b/src/demo.cpp index 7da6389..0628f1a 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -38,14 +38,14 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) pcl::fromROSMsg(*cloud_msg, pc_curr); - PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); + if (PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken)) { + ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() + << " (running_time: " << time_taken << " sec)" << "\033[0m"); - ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() - << " (running_time: " << time_taken << " sec)" << "\033[0m"); - - pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); - pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); - pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + } } int main(int argc, char**argv) { diff --git a/src/demo_reconf.cpp b/src/demo_reconf.cpp index c90b40c..13bb42d 100644 --- a/src/demo_reconf.cpp +++ b/src/demo_reconf.cpp @@ -47,14 +47,14 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) pcl::fromROSMsg(*cloud_msg, pc_curr); - PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); - - ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() - << " (running_time: " << time_taken << " sec)" << "\033[0m"); - - pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); - pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); - pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + if (PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken)) { + ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() + << " (running_time: " << time_taken << " sec)" << "\033[0m"); + + pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + } } int main(int argc, char**argv) { From d8031a8c886805f21c9b933da8bd41b2c9c838ef Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Fri, 26 May 2023 12:10:39 +0200 Subject: [PATCH 4/8] full support for dynamic_reconfigure --- CMakeLists.txt | 1 + cfg/patchwork.cfg | 2 +- include/patchworkpp/params.hpp | 128 +++++++++++++++++++--------- include/patchworkpp/patchworkpp.hpp | 96 ++++++++++++--------- include/patchworkpp/utils.hpp | 88 ++++++++++--------- launch/demo.launch | 2 +- launch/demo_reconf.launch | 7 +- package.xml | 1 - params/params.yaml | 4 +- src/demo.cpp | 5 +- src/demo_reconf.cpp | 49 +++++------ 11 files changed, 224 insertions(+), 159 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a4c8877..015bf17 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ add_dependencies(demo patchworkpp_generate_messages_cpp) add_executable(demo_reconf src/demo_reconf.cpp) target_link_libraries(demo_reconf ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) +target_compile_options(demo_reconf PRIVATE -Wsign-compare -Wunused-variable) add_dependencies(demo_reconf patchworkpp_generate_messages_cpp) add_executable(video src/video.cpp) diff --git a/cfg/patchwork.cfg b/cfg/patchwork.cfg index 28b75eb..d661024 100644 --- a/cfg/patchwork.cfg +++ b/cfg/patchwork.cfg @@ -8,6 +8,7 @@ gen.add("save_flag", bool_t, 0, "Flag to save the configuration file", True) # patchworkpp gen.add("sensor_height", double_t, 0, "Height of the sensor from the ground", 1.723, 0.0, 100.0) gen.add("mode", str_t, 0, "Mode of operation", "czm") +gen.add("cloud_topic", str_t, 0, "PointCloud Topic", "/kitti/velo/pointcloud") gen.add("verbose", bool_t, 0, "Verbose mode for debugging", False) gen.add("visualize", bool_t, 0, "Flag to visualize the ground likelihood estimation", True) @@ -25,7 +26,6 @@ gen.add("uprightness_thr", double_t, 0, "Threshold of uprightness using in Groun gen.add("adaptive_seed_selection_margin", double_t, 0, "The points below the adaptive_seed_selection_margin * sensor_height are filtered", -1.2, -10.0, 10.0) # czm -gen.add("czm_num_zones", int_t, 0, "Number of zones in czm mode", 4, 0, 10) gen.add("czm_num_sectors_each_zone", str_t, 0, "Number of sectors in each zone", "16, 32, 54, 32") gen.add("czm_num_rings_each_zone", str_t, 0, "Number of rings in each zone", "2, 4, 4, 4") gen.add("czm_elevation_thresholds", str_t, 0, "Threshold of elevation for each ring using in GLE. Those values are updated adaptively.", "0.0, 0.0, 0.0, 0.0") diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp index 6fb8fb2..c555877 100644 --- a/include/patchworkpp/params.hpp +++ b/include/patchworkpp/params.hpp @@ -11,16 +11,15 @@ class ParamsHandler { public: - ParamsHandler(std::recursive_mutex& mutex) : mutex_(mutex) { + ParamsHandler(std::recursive_mutex& mutex) : mutex_(mutex), czm_changed_(false), topic_changed_(false), czm() { + czm.num_zones_ = 4; auto f = boost::bind(&ParamsHandler::reconfigure_callback, this, _1, _2); server_.setCallback(f); - std::unique_lock<std::recursive_mutex> lock(mutex_); - validate(); } - ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex) { + ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex), czm() { nh.param("verbose", verbose_, false); - nh.param("verbose", visualize_, true); + nh.param("visualize", visualize_, true); nh.param("sensor_height", sensor_height_, 1.723); nh.param("num_iter", num_iter_, 3); nh.param("num_lpr", num_lpr_, 20); @@ -42,13 +41,15 @@ class ParamsHandler { nh.param("enable_TGR", enable_TGR_, true); // CZM denotes 'Concentric Zone Model'. Please refer to our paper - nh.getParam("czm/num_zones", czm.num_zones_); nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_); nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_); nh.getParam("czm/elevation_thresholds", czm.elevation_thr_); nh.getParam("czm/flatness_thresholds", czm.flatness_thr_); - validate(); + nh.param<std::string>("cloud_topic", cloud_topic_, "/pointcloud"); + + czm.num_zones_ = 4; + params_valid_ = validate(); num_rings_of_interest_ = czm.elevation_thr_.size(); @@ -65,22 +66,28 @@ class ParamsHandler { 2 * M_PI / czm.num_sectors_each_zone_.at(2), 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; - initialized_ = true; - } void print_params() const { + std::unique_lock<std::recursive_mutex> lock(mutex_); + if (not params_valid_) + { + ROS_WARN_STREAM("Can't print parameters. Not initialized"); + return; + } + ROS_INFO("Sensor Height: %f", sensor_height_); + ROS_INFO("Cloud topic: %s", cloud_topic_.c_str()); ROS_INFO("Num of Iteration: %d", num_iter_); ROS_INFO("Num of LPR: %d", num_lpr_); ROS_INFO("Num of min. points: %d", num_min_pts_); ROS_INFO("Seeds Threshold: %f", th_seeds_); ROS_INFO("Distance Threshold: %f", th_dist_); - ROS_INFO("Max. range:: %f", max_range_); - ROS_INFO("Min. range:: %f", min_range_); + ROS_INFO("Max. range: %f", max_range_); + ROS_INFO("Min. range: %f", min_range_); ROS_INFO("Normal vector threshold: %f", uprightness_thr_); ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); - ROS_INFO("Num. zones: %d", czm.num_zones_); + ROS_INFO("Num. zones: %ld", czm.num_zones_); ROS_INFO_STREAM((boost::format("Num. sectors: %d, %d, %d, %d") % czm.num_sectors_each_zone_[0] % czm.num_sectors_each_zone_[1] % czm.num_sectors_each_zone_[2] % czm.num_sectors_each_zone_[3]).str()); @@ -96,16 +103,39 @@ class ParamsHandler { czm.flatness_thr_[3]).str()); } - bool validate() { - return check(czm.num_zones_ != 4, "Number of zones must be four!") &&\ - check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones and length of num_sectors_each_zone must match") &&\ + bool validate() const { + return check(min_range_ > 0, "min range must be larger than 0") &&\ + check(min_range_ < max_range_, "min range must be smaller than max range") &&\ + check(max_range_ > 0, "max range must be larger than 0") &&\ + check(czm.num_zones_ == 4, (boost::format("Number of zones must be four! Got %d") % czm.num_zones_).str()) &&\ + check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones (4) and length of num_sectors_each_zone must match") &&\ check(czm.num_zones_ == czm.num_rings_each_zone_.size(), "num_zones and length of num_rings_each_zone must match") &&\ check(czm.num_zones_ == czm.elevation_thr_.size(), "num_zones and length of elevation_thresholds must match") &&\ check(czm.num_zones_ == czm.flatness_thr_.size(), "num_zones and length of flatness_thresholds must match"); } + bool czm_changed() { + std::unique_lock<std::recursive_mutex> lock(mutex_); + auto result = czm_changed_; + czm_changed_ = false; + return result; + } + + std::pair<bool, std::string> topic_changed() { + std::unique_lock<std::recursive_mutex> lock(mutex_); + auto result = topic_changed_; + topic_changed_ = false; + return { result, cloud_topic_ }; + } + + std::string topic() { + std::unique_lock<std::recursive_mutex> lock(mutex_); + return cloud_topic_; + } + std::string mode_; - bool initialized_; + std::string cloud_topic_; + bool params_valid_; bool verbose_; bool visualize_; double sensor_height_; @@ -134,7 +164,7 @@ class ParamsHandler { struct CZM { - int num_zones_; + size_t num_zones_; std::vector<int> num_sectors_each_zone_; std::vector<int> num_rings_each_zone_; std::vector<double> elevation_thr_; @@ -161,32 +191,51 @@ class ParamsHandler { enable_RNR_ = config.enable_RNR; enable_RVPF_ = config.enable_RVPF; enable_TGR_ = config.enable_TGR; + min_range_ = config.min_r; + max_range_ = config.max_r; - czm.num_zones_ = config.czm_num_zones; - czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone); - czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone); - czm.elevation_thr_ = convert_string_to_vector<double>(config.czm_elevation_thresholds); - czm.flatness_thr_ = convert_string_to_vector<double>(config.czm_flatness_thresholds); + if (cloud_topic_ != config.cloud_topic) { + topic_changed_ = true; + } + cloud_topic_ = config.cloud_topic; - num_rings_of_interest_ = czm.elevation_thr_.size(); + auto num_sectors_each_zone = convert_string_to_vector<int>(config.czm_num_sectors_each_zone); + auto num_rings_each_zone = convert_string_to_vector<int>(config.czm_num_rings_each_zone); + auto elevation_thr = convert_string_to_vector<double>(config.czm_elevation_thresholds); + auto flatness_thr = convert_string_to_vector<double>(config.czm_flatness_thresholds);; + if (czm.num_sectors_each_zone_ != num_sectors_each_zone || \ + czm.num_rings_each_zone_ != num_rings_each_zone || \ + czm.elevation_thr_ != elevation_thr || \ + czm.flatness_thr_ != flatness_thr) { + czm_changed_ = true; + } - auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; - auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; - auto min_range_z4 = (min_range_ + max_range_) / 2.0; + czm.num_sectors_each_zone_ = num_sectors_each_zone; + czm.num_rings_each_zone_ = num_rings_each_zone; + czm.elevation_thr_ = elevation_thr; + czm.flatness_thr_ = flatness_thr; - validate(); + params_valid_ = validate(); + if (params_valid_) + { + num_rings_of_interest_ = czm.elevation_thr_.size(); - min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; - ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), - (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), - (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), - (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), - 2 * M_PI / czm.num_sectors_each_zone_.at(2), - 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; + auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; + auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; + auto min_range_z4 = (min_range_ + max_range_) / 2.0; - ROS_INFO("Updated params"); - initialized_ = true; + min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; + ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), + (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), + (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), + (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; + sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), + 2 * M_PI / czm.num_sectors_each_zone_.at(2), + 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; + ROS_INFO("Updated params"); + } else { + ROS_WARN("Parameter update failed"); + } } @@ -205,13 +254,14 @@ class ParamsHandler { ROS_WARN_STREAM("Can't convert " << token << " to number"); return result; } + result.push_back(num); } return result; } - bool check(bool assertion, std::string description) { + bool check(bool assertion, std::string description) const { if (not assertion) { ROS_WARN_STREAM(description); return false; @@ -220,6 +270,8 @@ class ParamsHandler { return true; } + bool czm_changed_; + bool topic_changed_; std::recursive_mutex& mutex_; dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_; }; diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index eabc4a7..d427e49 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -46,7 +46,7 @@ bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; } template <typename PointT> struct RevertCandidate { - int concentric_idx; + size_t concentric_idx; int sector_idx; double ground_flatness; double line_variable; @@ -65,8 +65,6 @@ class PatchWorkpp { typedef std::vector<Ring> Zone; PatchWorkpp() : mutex_(), params_(mutex_), using_reconf_(true) { - params_.print_params(); - plane_viz_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true); pub_revert_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true); pub_reject_pc_ = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true); @@ -79,10 +77,13 @@ class PatchWorkpp { regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - reset_poly_list(); - reset_concentric_zone_model(); - - ROS_INFO("INITIALIZATION COMPLETE"); + if (params_.params_valid_) + { + params_.print_params(); + ROS_INFO("INITIALIZATION COMPLETE"); + reset_poly_list(); + reset_concentric_zone_model(); + } } PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), mutex_(), params_(nh, mutex_), using_reconf_(false) { @@ -109,6 +110,8 @@ class PatchWorkpp { bool estimate_ground(pcl::PointCloud<PointT> cloud_in, pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, double &time_taken); + std::pair<bool, std::string> topic_changed(); + std::string topic(); private: // Every private member variable is written with the undescore("_") in its end. @@ -161,7 +164,7 @@ class PatchWorkpp { void temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, std::vector<double> ring_flatness, std::vector<RevertCandidate<PointT>> candidates, - int concentric_idx); + size_t concentric_idx); void calc_mean_stdev(std::vector<double> vec, double &mean, double &stdev); @@ -194,7 +197,7 @@ class PatchWorkpp { void set_ground_likelihood_estimation_status( const int zone_idx, const int ring_idx, - const int concentric_idx, + const size_t concentric_idx, const double z_vec, const double z_elevation, const double ground_flatness); @@ -226,9 +229,9 @@ void PatchWorkpp<PointT>::flush_patches_in_zone(Zone &patches, int num_sectors, template<typename PointT> inline void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) { - for (int k = 0; k < params_.czm.num_zones_; k++) { - for (int i = 0; i < params_.czm.num_rings_each_zone_[k]; i++) { - for (int j = 0; j < params_.czm.num_sectors_each_zone_[k]; j++) { + for (size_t k = 0; k < params_.czm.num_zones_; k++) { + for (size_t i = 0; i < params_.czm.num_rings_each_zone_[k]; i++) { + for (size_t j = 0; j < params_.czm.num_sectors_each_zone_[k]; j++) { if (!czm[k][i][j].points.empty()) czm[k][i][j].points.clear(); } } @@ -248,7 +251,7 @@ void PatchWorkpp<PointT>::reset_poly_list() { template<typename PointT> inline void PatchWorkpp<PointT>::reset_concentric_zone_model() { ConcentricZoneModel_.clear(); - for (int i = 0; i < params_.czm.num_zones_; i++) { + for (size_t i = 0; i < params_.czm.num_zones_; i++) { Zone z; initialize_zone(z, params_.czm.num_sectors_each_zone_[i], params_.czm.num_rings_each_zone_[i]); ConcentricZoneModel_.push_back(z); @@ -284,9 +287,9 @@ void PatchWorkpp<PointT>::extract_initial_seeds( double sum = 0; int cnt = 0; - int init_idx = 0; + size_t init_idx = 0; if (zone_idx == 0) { - for (int i = 0; i < p_sorted.points.size(); i++) { + for (size_t i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) { ++init_idx; } else { @@ -296,14 +299,14 @@ void PatchWorkpp<PointT>::extract_initial_seeds( } // Calculate the mean height value. - for (int i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { + for (size_t i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_ - for (int i = 0; i < p_sorted.points.size(); i++) { + for (size_t i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lpr_height + th_seed) { init_seeds.points.push_back(p_sorted.points[i]); } @@ -322,7 +325,7 @@ void PatchWorkpp<PointT>::extract_initial_seeds( int init_idx = 0; if (zone_idx == 0) { - for (int i = 0; i < p_sorted.points.size(); i++) { + for (size_t i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) { ++init_idx; } else { @@ -332,14 +335,14 @@ void PatchWorkpp<PointT>::extract_initial_seeds( } // Calculate the mean height value. - for (int i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { + for (size_t i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_ - for (int i = 0; i < p_sorted.points.size(); i++) { + for (size_t i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lpr_height + params_.th_seeds_) { init_seeds.points.push_back(p_sorted.points[i]); } @@ -349,7 +352,7 @@ void PatchWorkpp<PointT>::extract_initial_seeds( template<typename PointT> inline void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_nonground) { - for (int i=0; i<cloud_in.size(); i++) + for (size_t i = 0; i < cloud_in.size(); i++) { double r = sqrt( cloud_in[i].x*cloud_in[i].x + cloud_in[i].y*cloud_in[i].y ); double z = cloud_in[i].z; @@ -366,6 +369,16 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud if (params_ .verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl; } +template<typename PointT> inline +std::pair<bool, std::string> PatchWorkpp<PointT>::topic_changed() { + return params_.topic_changed(); +} + +template<typename PointT> inline +std::string PatchWorkpp<PointT>::topic() { + return params_.topic(); +} + /* @brief Velodyne pointcloud callback function. The main GPF pipeline is here. PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud @@ -381,14 +394,15 @@ bool PatchWorkpp<PointT>::estimate_ground( unique_lock<recursive_mutex> lock(mutex_); - if (not params_.initialized_ || params_.num_iter_ < 1) { - ROS_WARN_STREAM("Waiting for initialization..."); + if (not params_.params_valid_) { + ROS_WARN_STREAM_THROTTLE(1, "Invalid Parameters..."); return false; } - if (using_reconf_ && not params_.validate()) { - ROS_WARN_STREAM("Invalid parameters"); - return false; + if (using_reconf_ && params_.czm_changed()) { + ROS_WARN_STREAM("Resetting poly list and concentric zone model"); + reset_poly_list(); + reset_concentric_zone_model(); } poly_list_.header.stamp = ros::Time::now(); @@ -396,7 +410,7 @@ bool PatchWorkpp<PointT>::estimate_ground( if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); - static double start, t0, t1, t2, end; + static double start, t1, t2, end; double pca_time_ = 0.0; double t_revert = 0.0; @@ -419,19 +433,19 @@ bool PatchWorkpp<PointT>::estimate_ground( t2 = ros::Time::now().toSec(); - int concentric_idx = 0; + size_t concentric_idx = 0; double t_sort = 0; std::vector<RevertCandidate<PointT>> candidates; std::vector<double> ringwise_flatness; - for (int zone_idx = 0; zone_idx < params_.czm.num_zones_; ++zone_idx) { + for (size_t zone_idx = 0; zone_idx < params_.czm.num_zones_; ++zone_idx) { auto zone = ConcentricZoneModel_[zone_idx]; - for (int ring_idx = 0; ring_idx < params_.czm.num_rings_each_zone_[zone_idx]; ++ring_idx) { - for (int sector_idx = 0; sector_idx < params_.czm.num_sectors_each_zone_[zone_idx]; ++sector_idx) { + for (size_t ring_idx = 0; ring_idx < params_.czm.num_rings_each_zone_[zone_idx]; ++ring_idx) { + for (size_t sector_idx = 0; sector_idx < params_.czm.num_sectors_each_zone_[zone_idx]; ++sector_idx) { if (zone[ring_idx][sector_idx].points.size() < params_.num_min_pts_) { @@ -463,7 +477,9 @@ bool PatchWorkpp<PointT>::estimate_ground( const double line_variable = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits<double>::max(); double heading = 0.0; - for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i); + for (size_t i = 0; i < 3; i++) { + heading += pc_mean_(i,0)*normal_(i); + } if (params_.visualize_) { auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3); @@ -551,7 +567,7 @@ bool PatchWorkpp<PointT>::estimate_ground( } else { - for (size_t i=0; i<candidates.size(); i++) + for (size_t i = 0; i < candidates.size(); i++) { cloud_nonground += candidates[i].regionwise_ground; } @@ -569,8 +585,6 @@ bool PatchWorkpp<PointT>::estimate_ground( } } - double t_update = ros::Time::now().toSec(); - update_elevation_thr(); update_flatness_thr(); @@ -630,7 +644,7 @@ bool PatchWorkpp<PointT>::estimate_ground( template<typename PointT> inline void PatchWorkpp<PointT>::update_elevation_thr(void) { - for (int i=0; i<params_.num_rings_of_interest_; i++) + for (size_t i=0; i < params_.num_rings_of_interest_; i++) { if (update_elevation_[i].empty()) continue; @@ -661,7 +675,7 @@ void PatchWorkpp<PointT>::update_elevation_thr(void) template<typename PointT> inline void PatchWorkpp<PointT>::update_flatness_thr(void) { - for (int i=0; i<params_.num_rings_of_interest_; i++) + for (size_t i=0; i < params_.num_rings_of_interest_; i++) { if (update_flatness_[i].empty()) break; if (update_flatness_[i].size() <= 1) break; @@ -688,7 +702,7 @@ void PatchWorkpp<PointT>::update_flatness_thr(void) template<typename PointT> inline void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, std::vector<double> ring_flatness, std::vector<RevertCandidate<PointT>> candidates, - int concentric_idx) + size_t concentric_idx) { if (params_ .verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl; @@ -743,7 +757,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ } else { - if (params_ .verbose_) + if (params_.verbose_) { cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl; } @@ -897,7 +911,7 @@ geometry_msgs::PolygonStamped PatchWorkpp<PointT>::set_polygons(int zone_idx, in template<typename PointT> inline void PatchWorkpp<PointT>::set_ground_likelihood_estimation_status( const int zone_idx, const int ring_idx, - const int concentric_idx, + const size_t concentric_idx, const double z_vec, const double z_elevation, const double ground_flatness) { @@ -927,7 +941,7 @@ void PatchWorkpp<PointT>::calc_mean_stdev(std::vector<double> vec, double &mean, mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size(); - for (int i=0; i<vec.size(); i++) { stdev += (vec.at(i)-mean)*(vec.at(i)-mean); } + for (size_t i=0; i < vec.size(); i++) { stdev += (vec.at(i)-mean)*(vec.at(i)-mean); } stdev /= vec.size()-1; stdev = sqrt(stdev); } diff --git a/include/patchworkpp/utils.hpp b/include/patchworkpp/utils.hpp index 9230b19..2f6115b 100755 --- a/include/patchworkpp/utils.hpp +++ b/include/patchworkpp/utils.hpp @@ -70,7 +70,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID, void PointXYZILID2XYZI(pcl::PointCloud<PointXYZILID>& src, - pcl::PointCloud<pcl::PointXYZI>::Ptr dst){ + pcl::PointCloud<pcl::PointXYZI>::Ptr dst) { dst->points.clear(); for (const auto &pt: src.points){ pcl::PointXYZI pt_xyzi; @@ -86,7 +86,7 @@ std::vector<int> ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_ std::vector<int> ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING}; std::vector<int> traversable_ground_classes = {ROAD, PARKING, LANE_MARKING, OTHER_GROUND}; -int count_num_ground(const pcl::PointCloud<PointXYZILID>& pc){ +int count_num_ground(const pcl::PointCloud<PointXYZILID>& pc) { int num_ground = 0; std::vector<int>::iterator iter; @@ -94,17 +94,19 @@ int count_num_ground(const pcl::PointCloud<PointXYZILID>& pc){ for (auto const& pt: pc.points){ iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes - if (pt.label == VEGETATION){ - if (pt.z < VEGETATION_THR){ + if (pt.label == VEGETATION) { + if (pt.z < VEGETATION_THR) { num_ground ++; } - }else num_ground ++; + } else { + num_ground ++; + } } } return num_ground; } -int count_num_ground_without_vegetation(const pcl::PointCloud<PointXYZILID>& pc){ +int count_num_ground_without_vegetation(const pcl::PointCloud<PointXYZILID>& pc) { int num_ground = 0; std::vector<int>::iterator iter; @@ -113,34 +115,35 @@ int count_num_ground_without_vegetation(const pcl::PointCloud<PointXYZILID>& pc) for (auto const& pt: pc.points){ iter = std::find(classes.begin(), classes.end(), pt.label); - if (iter != classes.end()){ // corresponding class is in ground classes + if (iter != classes.end()) { // corresponding class is in ground classes num_ground ++; } } return num_ground; } -std::map<int, int> set_initial_gt_counts(std::vector<int>& gt_classes){ +std::map<int, int> set_initial_gt_counts(std::vector<int>& gt_classes) { map<int, int> gt_counts; - for (int i = 0; i< gt_classes.size(); ++i){ + for (size_t i = 0; i < gt_classes.size(); ++i) { gt_counts.insert(pair<int,int>(gt_classes.at(i), 0)); } return gt_counts; } -std::map<int, int> count_num_each_class(const pcl::PointCloud<PointXYZILID>& pc){ - int num_ground = 0; +std::map<int, int> count_num_each_class(const pcl::PointCloud<PointXYZILID>& pc) { auto gt_counts = set_initial_gt_counts(ground_classes); std::vector<int>::iterator iter; for (auto const& pt: pc.points){ iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes - if (pt.label == VEGETATION){ - if (pt.z < VEGETATION_THR){ + if (pt.label == VEGETATION) { + if (pt.z < VEGETATION_THR) { gt_counts.find(pt.label)->second++; } - }else gt_counts.find(pt.label)->second++; + } else { + gt_counts.find(pt.label)->second++; + } } } return gt_counts; @@ -165,16 +168,20 @@ void discern_ground(const pcl::PointCloud<PointXYZILID>& src, pcl::PointCloud<Po ground.clear(); non_ground.clear(); std::vector<int>::iterator iter; - for (auto const& pt: src.points){ + for (auto const& pt: src.points) { if (pt.label == UNLABELED || pt.label == OUTLIER) continue; iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes - if (pt.label == VEGETATION){ - if (pt.z < VEGETATION_THR){ + if (pt.label == VEGETATION) { + if (pt.z < VEGETATION_THR) { ground.push_back(pt); - }else non_ground.push_back(pt); - }else ground.push_back(pt); - }else{ + } else { + non_ground.push_back(pt); + } + } else { + ground.push_back(pt); + } + } else { non_ground.push_back(pt); } } @@ -184,12 +191,12 @@ void discern_ground_without_vegetation(const pcl::PointCloud<PointXYZILID>& src, ground.clear(); non_ground.clear(); std::vector<int>::iterator iter; - for (auto const& pt: src.points){ + for (auto const& pt: src.points) { if (pt.label == UNLABELED || pt.label == OUTLIER) continue; iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes if (pt.label != VEGETATION) ground.push_back(pt); - }else{ + } else { non_ground.push_back(pt); } } @@ -200,16 +207,16 @@ void calculate_precision_recall(const pcl::PointCloud<PointXYZILID>& pc_curr, pcl::PointCloud<PointXYZILID>& ground_estimated, double & precision, double& recall, - bool consider_outliers=true){ + bool consider_outliers=true) { int num_ground_est = ground_estimated.points.size(); int num_ground_gt = count_num_ground(pc_curr); int num_TP = count_num_ground(ground_estimated); - if (consider_outliers){ + if (consider_outliers) { int num_outliers_est = count_num_outliers(ground_estimated); precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; recall = (double)(num_TP)/num_ground_gt * 100; - }else{ + } else { precision = (double)(num_TP)/num_ground_est * 100; recall = (double)(num_TP)/num_ground_gt * 100; } @@ -219,28 +226,27 @@ void calculate_precision_recall_without_vegetation(const pcl::PointCloud<PointXY pcl::PointCloud<PointXYZILID>& ground_estimated, double & precision, double& recall, - bool consider_outliers=true){ + bool consider_outliers=true) { int num_veg = 0; - for (auto const& pt: ground_estimated.points) - { + for (auto const& pt: ground_estimated.points) { if (pt.label == VEGETATION) num_veg++; } int num_ground_est = ground_estimated.size() - num_veg; int num_ground_gt = count_num_ground_without_vegetation(pc_curr); int num_TP = count_num_ground_without_vegetation(ground_estimated); - if (consider_outliers){ + if (consider_outliers) { int num_outliers_est = count_num_outliers(ground_estimated); precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; recall = (double)(num_TP)/num_ground_gt * 100; - }else{ + } else { precision = (double)(num_TP)/num_ground_est * 100; recall = (double)(num_TP)/num_ground_gt * 100; } } -void save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_DIR, string seq, int count){ +void save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_DIR, string seq, int count) { std::string count_str = std::to_string(count); std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; @@ -285,10 +291,10 @@ void save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_DIR, st else if (pt.label == 259) labels[33]++; } - for (uint8_t i=0; i < NUM_ALL_CLASSES;++i){ - if (i!=33){ + for (uint8_t i=0; i < NUM_ALL_CLASSES;++i) { + if (i!=33) { sc_output<<labels[i]<<","; - }else{ + } else { sc_output<<labels[i]<<endl; } } @@ -297,7 +303,7 @@ void save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_DIR, st void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& pc_curr, pcl::PointCloud<PointXYZILID>& ground_estimated, string acc_filename, - double& accuracy, map<int, int>&pc_curr_gt_counts, map<int, int>&g_est_gt_counts){ + double& accuracy, map<int, int>&pc_curr_gt_counts, map<int, int>&g_est_gt_counts) { // std::cout<<"debug: "<<acc_filename<<std::endl; @@ -319,7 +325,7 @@ void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& pc_curr, g_est_gt_counts = count_num_each_class(ground_estimated); // save output - for (auto const& class_id: ground_classes){ + for (auto const& class_id: ground_classes) { sc_output2<<g_est_gt_counts.find(class_id)->second<<","<<pc_curr_gt_counts.find(class_id)->second<<","; } sc_output2<<accuracy<<endl; @@ -329,28 +335,28 @@ void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& pc_curr, void pc2pcdfile(const pcl::PointCloud<PointXYZILID>& TP, const pcl::PointCloud<PointXYZILID>& FP, const pcl::PointCloud<PointXYZILID>& FN, const pcl::PointCloud<PointXYZILID>& TN, - std::string pcd_filename){ + std::string pcd_filename) { pcl::PointCloud<pcl::PointXYZI> pc_out; - for (auto const pt: TP.points){ + for (auto const pt: TP.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = TRUEPOSITIVE; pc_out.points.push_back(pt_est); } - for (auto const pt: FP.points){ + for (auto const pt: FP.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = FALSEPOSITIVE; pc_out.points.push_back(pt_est); } - for (auto const pt: FN.points){ + for (auto const pt: FN.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = FALSENEGATIVE; pc_out.points.push_back(pt_est); } - for (auto const pt: TN.points){ + for (auto const pt: TN.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = TRUENEGATIVE; diff --git a/launch/demo.launch b/launch/demo.launch index 38d2129..bc6a1be 100755 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -1,6 +1,6 @@ <launch> -<arg name="cloud_topic" default="/velodyne_points"/> +<arg name="cloud_topic" default="/kitti/velo/pointcloud"/> <node name="ground_segmentation" pkg="patchworkpp" type="demo" output="screen"> <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" /> diff --git a/launch/demo_reconf.launch b/launch/demo_reconf.launch index 3cf4554..8484730 100755 --- a/launch/demo_reconf.launch +++ b/launch/demo_reconf.launch @@ -1,13 +1,8 @@ <launch> - <arg name="cloud_topic" default="/velodyne_points"/> - <node name="rqt_conf_gui" pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen"/> - <node name="ground_segmentation" pkg="patchworkpp" type="demo_reconf" output="screen"> - <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" /> - <param name="cloud_topic" value="$(arg cloud_topic)"/> - </node> + <node name="ground_segmentation" pkg="patchworkpp" type="demo_reconf" output="screen"/> <node name="$(anon rviz)" pkg="rviz" type="rviz" args="-d $(find patchworkpp)/rviz/demo.rviz"/> diff --git a/package.xml b/package.xml index ed6916a..863b0ae 100755 --- a/package.xml +++ b/package.xml @@ -31,7 +31,6 @@ <build_depend>jsk_recognition_msgs</build_depend> <build_depend>laser_geometry</build_depend> - <!-- The export tag contains other, unspecified, tags --> <export> <!-- Other tools can request additional information be placed here --> diff --git a/params/params.yaml b/params/params.yaml index b3f494f..46c1e2a 100755 --- a/params/params.yaml +++ b/params/params.yaml @@ -2,7 +2,8 @@ save_flag: true # patchworkpp: -sensor_height: 1.2 +sensor_height: 1.723 +point_topic: /kitti/velo/pointcloud mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness @@ -22,7 +23,6 @@ uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Es 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] num_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. diff --git a/src/demo.cpp b/src/demo.cpp index 0628f1a..88af7dc 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -54,9 +54,6 @@ int main(int argc, char**argv) { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - std::string cloud_topic; - pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud"); - ROS_INFO("Operating patchwork++..."); PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh)); @@ -64,7 +61,7 @@ int main(int argc, char**argv) { pub_ground = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true); pub_non_ground = pnh.advertise<sensor_msgs::PointCloud2>("nonground", 100, true); - ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud); + ros::Subscriber sub_cloud = nh.subscribe(PatchworkppGroundSeg->topic(), 100, callbackCloud); while (ros::ok()) diff --git a/src/demo_reconf.cpp b/src/demo_reconf.cpp index 13bb42d..9cae49a 100644 --- a/src/demo_reconf.cpp +++ b/src/demo_reconf.cpp @@ -3,6 +3,7 @@ #define PCL_NO_PRECOMPILE #include <ros/ros.h> +#include <ros/master.h> #include <signal.h> #include <dynamic_reconfigure/server.h> #include <sensor_msgs/PointCloud2.h> @@ -27,16 +28,6 @@ sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, const ros::Time& st return cloud_ROS; } -// Callback function for parameter changes -void reconfigureCallback(const patchworkpp::PatchworkppConfig& config, uint32_t level) -{ - // Update your parameters based on the changes - // You can access the parameters using config.param_name - // For example: int parameter = config.int_param; - // Your code to handle parameter changes goes here - ROS_INFO("Updated parameters"); -} - void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) { double time_taken; @@ -63,9 +54,6 @@ int main(int argc, char**argv) { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - std::string cloud_topic; - pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud"); - ROS_INFO("Operating patchwork++..."); PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>()); @@ -73,17 +61,30 @@ int main(int argc, char**argv) { pub_ground = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true); pub_non_ground = pnh.advertise<sensor_msgs::PointCloud2>("nonground", 100, true); - ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud); - - // Create a dynamic reconfigure server - dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server; - dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig>::CallbackType f; - - // Set the callback function - f = boost::bind(&reconfigureCallback, _1, _2); - server.setCallback(f); - - ros::spin(); + ros::Subscriber sub_cloud = nh.subscribe(PatchworkppGroundSeg->topic(), 100, callbackCloud); + + while (ros::ok()) + { + auto [topic_changed, topic] = PatchworkppGroundSeg->topic_changed(); + if (topic_changed) { + std::vector<ros::master::TopicInfo> topics; + if (ros::master::check()) { + ros::master::getTopics(topics); + + bool topic_exists = std::find_if(topics.begin(), topics.end(), [&topic](const auto& topic_info) { return topic_info.name == topic; }) != topics.end(); + + if (topic_exists) + { + sub_cloud = nh.subscribe(PatchworkppGroundSeg->topic(), 100, callbackCloud); + } + else + { + ROS_WARN_STREAM("Topic " << topic << " doesn't exist"); + } + } + } + ros::spinOnce(); + } return 0; } From 0fc543ceaa2ca6751f894e1569655eb4a8b62542 Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Fri, 26 May 2023 12:24:35 +0200 Subject: [PATCH 5/8] fix sign issues --- CMakeLists.txt | 1 - include/patchworkpp/params.hpp | 28 +++++++++++++++++----------- include/patchworkpp/patchworkpp.hpp | 10 +++++----- 3 files changed, 22 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 015bf17..a4c8877 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,7 +76,6 @@ add_dependencies(demo patchworkpp_generate_messages_cpp) add_executable(demo_reconf src/demo_reconf.cpp) target_link_libraries(demo_reconf ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) -target_compile_options(demo_reconf PRIVATE -Wsign-compare -Wunused-variable) add_dependencies(demo_reconf patchworkpp_generate_messages_cpp) add_executable(video src/video.cpp) diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp index c555877..941608e 100644 --- a/include/patchworkpp/params.hpp +++ b/include/patchworkpp/params.hpp @@ -23,7 +23,9 @@ class ParamsHandler { nh.param("sensor_height", sensor_height_, 1.723); nh.param("num_iter", num_iter_, 3); nh.param("num_lpr", num_lpr_, 20); - nh.param("num_min_pts", num_min_pts_, 10); + int tmp = 0; + nh.param("num_min_pts", tmp, 10); + num_min_pts_ = tmp; nh.param("th_seeds", th_seeds_, 0.4); nh.param("th_dist", th_dist_, 0.3); nh.param("th_seeds_v", th_seeds_v_, 0.4); @@ -41,8 +43,12 @@ class ParamsHandler { nh.param("enable_TGR", enable_TGR_, true); // CZM denotes 'Concentric Zone Model'. Please refer to our paper - nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_); - nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_); + std::vector<int> temp; + nh.getParam("czm/num_sectors_each_zone", temp); + std::transform(temp.begin(), temp.end(), std::back_inserter(czm.num_sectors_each_zone_), [](int value) { return static_cast<size_t>(value); }); + temp.clear(); + nh.getParam("czm/num_rings_each_zone", temp); + std::transform(temp.begin(), temp.end(), std::back_inserter(czm.num_rings_each_zone_), [](int value) { return static_cast<size_t>(value); }); nh.getParam("czm/elevation_thresholds", czm.elevation_thr_); nh.getParam("czm/flatness_thresholds", czm.flatness_thr_); @@ -80,7 +86,7 @@ class ParamsHandler { ROS_INFO("Cloud topic: %s", cloud_topic_.c_str()); ROS_INFO("Num of Iteration: %d", num_iter_); ROS_INFO("Num of LPR: %d", num_lpr_); - ROS_INFO("Num of min. points: %d", num_min_pts_); + ROS_INFO("Num of min. points: %ld", num_min_pts_); ROS_INFO("Seeds Threshold: %f", th_seeds_); ROS_INFO("Distance Threshold: %f", th_dist_); ROS_INFO("Max. range: %f", max_range_); @@ -88,10 +94,10 @@ class ParamsHandler { ROS_INFO("Normal vector threshold: %f", uprightness_thr_); ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); ROS_INFO("Num. zones: %ld", czm.num_zones_); - ROS_INFO_STREAM((boost::format("Num. sectors: %d, %d, %d, %d") % czm.num_sectors_each_zone_[0] % czm.num_sectors_each_zone_[1] % + ROS_INFO_STREAM((boost::format("Num. sectors: %ld, %ld, %ld, %ld") % czm.num_sectors_each_zone_[0] % czm.num_sectors_each_zone_[1] % czm.num_sectors_each_zone_[2] % czm.num_sectors_each_zone_[3]).str()); - ROS_INFO_STREAM((boost::format("Num. rings: %01d, %01d, %01d, %01d") % czm.num_rings_each_zone_[0] % + ROS_INFO_STREAM((boost::format("Num. rings: %01ld, %01ld, %01ld, %01ld") % czm.num_rings_each_zone_[0] % czm.num_rings_each_zone_[1] % czm.num_rings_each_zone_[2] % czm.num_rings_each_zone_[3]).str()); @@ -141,7 +147,7 @@ class ParamsHandler { double sensor_height_; int num_iter_; int num_lpr_; - int num_min_pts_; + size_t num_min_pts_; double th_seeds_; double th_dist_; double th_seeds_v_; @@ -165,8 +171,8 @@ class ParamsHandler { struct CZM { size_t num_zones_; - std::vector<int> num_sectors_each_zone_; - std::vector<int> num_rings_each_zone_; + std::vector<size_t> num_sectors_each_zone_; + std::vector<size_t> num_rings_each_zone_; std::vector<double> elevation_thr_; std::vector<double> flatness_thr_; } czm; @@ -199,8 +205,8 @@ class ParamsHandler { } cloud_topic_ = config.cloud_topic; - auto num_sectors_each_zone = convert_string_to_vector<int>(config.czm_num_sectors_each_zone); - auto num_rings_each_zone = convert_string_to_vector<int>(config.czm_num_rings_each_zone); + auto num_sectors_each_zone = convert_string_to_vector<size_t>(config.czm_num_sectors_each_zone); + auto num_rings_each_zone = convert_string_to_vector<size_t>(config.czm_num_rings_each_zone); auto elevation_thr = convert_string_to_vector<double>(config.czm_elevation_thresholds); auto flatness_thr = convert_string_to_vector<double>(config.czm_flatness_thresholds);; if (czm.num_sectors_each_zone_ != num_sectors_each_zone || \ diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index d427e49..cbf3a50 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -135,7 +135,7 @@ class PatchWorkpp { Eigen::Matrix3f cov_; Eigen::Vector4f pc_mean_; - queue<int> noise_idxs_; + queue<size_t> noise_idxs_; vector<Zone> ConcentricZoneModel_; @@ -966,8 +966,8 @@ double PatchWorkpp<PointT>::xy2radius(const double &x, const double &y) { template<typename PointT> inline void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector<Zone> &czm, pcl::PointCloud<PointT> &cloud_nonground) { - for (int i=0; i<src.size(); i++) { - if ((!noise_idxs_.empty()) &&(i == noise_idxs_.front())) { + for (size_t i = 0; i < src.size(); i++) { + if ((!noise_idxs_.empty()) && (i == noise_idxs_.front())) { noise_idxs_.pop(); continue; } @@ -984,8 +984,8 @@ void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector else if ( r < params_.min_ranges_[3] ) zone_idx = 2; else zone_idx = 3; - int ring_idx = min(static_cast<int>(((r - params_.min_ranges_[zone_idx]) / params_.ring_sizes_[zone_idx])), params_.czm.num_rings_each_zone_[zone_idx] - 1); - int sector_idx = min(static_cast<int>((theta / params_.sector_sizes_[zone_idx])), params_.czm.num_sectors_each_zone_[zone_idx] - 1); + size_t ring_idx = min(static_cast<size_t>(((r - params_.min_ranges_[zone_idx]) / params_.ring_sizes_[zone_idx])), params_.czm.num_rings_each_zone_[zone_idx] - 1); + size_t sector_idx = min(static_cast<size_t>((theta / params_.sector_sizes_[zone_idx])), params_.czm.num_sectors_each_zone_[zone_idx] - 1); czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt); } From 1a7512a964adea910220dfb23e465e1785047e56 Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Fri, 26 May 2023 12:34:07 +0200 Subject: [PATCH 6/8] update instructions --- README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.md b/README.md index 9d34987..3bc66cf 100644 --- a/README.md +++ b/README.md @@ -57,6 +57,15 @@ $ roslaunch patchworkpp demo.launch $ rosbag play kitti_00_sample.bag ``` +or run the demo that is dynamically reconfigurable with `rqt_reconfigure` + +```bash +# Start reconfigurable Patchwork++ +$ roslaunch patchworkpp demo_reconf.launch +# Start the bag file +$ rosbag play kitti_00_sample.bag +``` + ## :pushpin: TODO List - [ ] Update additional demo codes processing data with .bin file format - [ ] Generalize point type in the source code From 6c25aeb848f2583c1f711ee4978ede1f9b5056a0 Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Fri, 26 May 2023 12:39:01 +0200 Subject: [PATCH 7/8] no redundant setting of c++ standard in cmake --- CMakeLists.txt | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a4c8877..3c46c38 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,20 +1,17 @@ -option(BUILD_JSK_PKGS "Enable building of required components of jsk_recognition_msgs and jsk_rviz_plugins" ON) - -if(BUILD_JSK_PKGS) - add_subdirectory(include/jsk_recognition_msgs) -# add_subdirectory(include/jsk_rviz_plugins) #TODO: allow building of rviz plugins as well -endif() - cmake_minimum_required(VERSION 3.0.2) project(patchworkpp) -add_compile_options(-std=c++17) set(CMAKE_BUILD_TYPE "Release") - -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +option(BUILD_JSK_PKGS "Enable building of required components of jsk_recognition_msgs and jsk_rviz_plugins" ON) +if(BUILD_JSK_PKGS) + add_subdirectory(include/jsk_recognition_msgs) +# add_subdirectory(include/jsk_rviz_plugins) #TODO: allow building of rviz plugins as well +endif() + find_package(catkin REQUIRED COMPONENTS roscpp rospy From 87621dfeb89082dd8798c3d847c53dc3a3642025 Mon Sep 17 00:00:00 2001 From: Julian Gaal <julian.gaal@anavs.de> Date: Fri, 26 May 2023 12:48:19 +0200 Subject: [PATCH 8/8] share code to set ranges, rings and sectors between reconf/!reconf --- include/patchworkpp/params.hpp | 51 ++++++++++++++-------------------- 1 file changed, 21 insertions(+), 30 deletions(-) diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp index 941608e..4598fa3 100644 --- a/include/patchworkpp/params.hpp +++ b/include/patchworkpp/params.hpp @@ -56,22 +56,9 @@ class ParamsHandler { czm.num_zones_ = 4; params_valid_ = validate(); - - num_rings_of_interest_ = czm.elevation_thr_.size(); - - auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; - auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; - auto min_range_z4 = (min_range_ + max_range_) / 2.0; - - min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; - ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), - (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), - (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), - (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), - 2 * M_PI / czm.num_sectors_each_zone_.at(2), - 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; - + if (params_valid_) { + set_ranges_rings_sectors(); + } } void print_params() const { @@ -224,20 +211,7 @@ class ParamsHandler { params_valid_ = validate(); if (params_valid_) { - num_rings_of_interest_ = czm.elevation_thr_.size(); - - auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; - auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; - auto min_range_z4 = (min_range_ + max_range_) / 2.0; - - min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; - ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), - (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), - (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), - (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), - 2 * M_PI / czm.num_sectors_each_zone_.at(2), - 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; + set_ranges_rings_sectors(); ROS_INFO("Updated params"); } else { ROS_WARN("Parameter update failed"); @@ -266,6 +240,23 @@ class ParamsHandler { return result; } + void set_ranges_rings_sectors() { + num_rings_of_interest_ = czm.elevation_thr_.size(); + + auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; + auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; + auto min_range_z4 = (min_range_ + max_range_) / 2.0; + + min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; + ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), + (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), + (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), + (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; + sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), + 2 * M_PI / czm.num_sectors_each_zone_.at(2), + 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; + } + bool check(bool assertion, std::string description) const { if (not assertion) {