Dynamic parameter changes with rqt_reconfigure

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
@@ -27,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
         laser_geometry
         sensor_msgs
         message_generation
+        dynamic_reconfigure
         jsk_recognition_msgs
 )
 
@@ -48,6 +46,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 +71,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_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})
 add_dependencies(video patchworkpp_generate_messages_cpp)

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

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("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)
+
+# Ground Plane Fitting parameters
+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)
+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_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"))

include/patchworkpp/params.hpp

@@ -0,0 +1,276 @@
+#ifndef PATCHWORKPP_PARAMS
+#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 ParamsHandler {
+public:
+    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);
+    }
+
+    ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex), czm() {
+        nh.param("verbose", verbose_, false);
+        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);
+        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);
+        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
+        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_);
+
+        nh.param<std::string>("cloud_topic", cloud_topic_, "/pointcloud");
+
+        czm.num_zones_ = 4;
+        params_valid_ = validate();
+        if (params_valid_) {
+            set_ranges_rings_sectors();
+        }
+    }
+
+    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: %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_);
+        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: %ld", czm.num_zones_);
+        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: %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());
+        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() 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_;
+    std::string cloud_topic_;
+    bool params_valid_;
+    bool verbose_;
+    bool visualize_;
+    double sensor_height_;
+    int num_iter_;
+    int num_lpr_;
+    size_t 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
+    {
+        size_t num_zones_;
+        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;
+
+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;
+        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;
+        min_range_ = config.min_r;
+        max_range_ = config.max_r;
+
+        if (cloud_topic_ != config.cloud_topic) {
+            topic_changed_ = true;
+        }
+        cloud_topic_ = config.cloud_topic;
+
+        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 || \
+            czm.num_rings_each_zone_ != num_rings_each_zone || \
+            czm.elevation_thr_ != elevation_thr || \
+            czm.flatness_thr_ != flatness_thr) {
+            czm_changed_ = true;
+        }
+
+        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;
+
+        params_valid_ = validate();
+        if (params_valid_)
+        {
+            set_ranges_rings_sectors();
+            ROS_INFO("Updated params");
+        } else {
+            ROS_WARN("Parameter update failed");
+        }
+    }
+
+
+    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;
+            }
+            result.push_back(num);
+        }
+
+        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) {
+            ROS_WARN_STREAM(description);
+            return false;
+        }
+
+        return true;
+    }
+
+    bool czm_changed_;
+    bool topic_changed_;
+    std::recursive_mutex& mutex_;
+    dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_;
+};
+
+#endif