pcodar: Add dynreconfig parameters for intensity filter bounds, add c…

…onfig files for real-life and sim
uf-mil · Nov 5, 2024 · 5c15f16 · 5c15f16
1 parent 56bd1f7
commit 5c15f16
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 8 deletions.
diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar.yaml
@@ -21,5 +21,9 @@ ogrid_width_meters: 500
 ogrid_resolution_meters_per_cell: 0.3
 ogrid_inflation_meters: 0.8
 
+# Intensity filter
+intensity_filter_min_intensity: 10
+intensity_filter_max_intensity: 100
+
 # yamllint disable-line rule:line-length
 classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder", "UNKNOWN"]
diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml
@@ -12,15 +12,19 @@ cluster_min_points: 5
 cluster_max_points: 1000
 
 # Associator
-associator_max_distance: 2.0
-associator_forget_unseen: false
+associator_max_distance: 20.0
+associator_forget_unseen: true
 
 # Ogrid
 ogrid_height_meters: 500
 ogrid_width_meters: 500
 ogrid_resolution_meters_per_cell: 0.3
 ogrid_inflation_meters: 0.8
 
+# Intensity filter
+intensity_filter_min_intensity: 0
+intensity_filter_max_intensity: 1
+
 # yamllint disable-line rule:line-length
 # classifications: ["buoy", "dock", "stc_platform", "red_totem", "green_totem", "blue_totem", "yellow_totem", "black_totem", "surmark46104", "surmark950400", "surmark950410", "UNKNOWN"]
 # yamllint disable-line rule:line-length

diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg b/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg
@@ -26,4 +26,8 @@ gen.add("ogrid_width_meters", double_t, 16, "", 1000, 1, 10000)
 gen.add("ogrid_resolution_meters_per_cell", double_t, 16, "", 0.3, 0., 10.)
 gen.add("ogrid_inflation_meters", double_t, 16, "", 2, 0., 10.)
 
+# Intensity filter
+gen.add("intensity_filter_min_intensity", double_t, 32, "", 10.0, 0., 1000.)
+gen.add("intensity_filter_max_intensity", double_t, 32, "", 100.0, 0., 1000.)
+
 exit(gen.generate("point_cloud_object_detection_and_recognition", "pcodar", "PCODAR"))
diff --git a/...nd_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp b/...nd_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp
@@ -82,6 +82,10 @@ class NodeBase
   // Visualization
   MarkerManager marker_manager_;
   OgridManager ogrid_manager_;
+
+  // Intensity filter
+  double intensity_filter_min_intensity;
+  double intensity_filter_max_intensity;
 };
 
 class Node : public NodeBase
@@ -96,6 +100,7 @@ class Node : public NodeBase
 private:
   bool bounds_update_cb(const mil_bounds::BoundsConfig& config) override;
   void ConfigCallback(Config const& config, uint32_t level) override;
+  void update_config(Config const& config);
   /// Reset PCODAR
   bool Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) override;
 

diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp
@@ -15,6 +15,8 @@ NodeBase::NodeBase(ros::NodeHandle _nh)
   , tf_listener(tf_buffer_, nh_)
   , global_frame_("enu")
   , config_server_(_nh)
+  , intensity_filter_min_intensity(10)
+  , intensity_filter_max_intensity(100)
   , objects_(std::make_shared<ObjectMap>())
 {
   config_server_.setCallback(std::bind(&NodeBase::ConfigCallback, this, std::placeholders::_1, std::placeholders::_2));
@@ -125,6 +127,12 @@ Node::Node(ros::NodeHandle _nh) : NodeBase(_nh)
   input_cloud_filter_.set_robot_footprint(min, max);
 }
 
+void Node::update_config(Config const& config)
+{
+  this->intensity_filter_min_intensity = config.intensity_filter_min_intensity;
+  this->intensity_filter_max_intensity = config.intensity_filter_max_intensity;
+}
+
 void Node::ConfigCallback(Config const& config, uint32_t level)
 {
   NodeBase::ConfigCallback(config, level);
@@ -136,6 +144,8 @@ void Node::ConfigCallback(Config const& config, uint32_t level)
     detector_.update_config(config);
   if (!level || level & 8)
     ass.update_config(config);
+  if (!level || level & 32)
+    this->update_config(config);
 }
 
 void Node::initialize()
@@ -171,14 +181,14 @@ void Node::velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud)
   if (!transform_point_cloud(*pcloud, *pc))
     return;
 
-  // our new filter vvv
-  pcl::PassThrough<pointi_t> _temp_intensity_filter;
-  _temp_intensity_filter.setInputCloud(pc);
-  _temp_intensity_filter.setFilterFieldName("intensity");
-  _temp_intensity_filter.setFilterLimits(10, 100);
+  // Intensity filter
+  pcl::PassThrough<pointi_t> _intensity_filter;
+  _intensity_filter.setInputCloud(pc);
+  _intensity_filter.setFilterFieldName("intensity");
+  _intensity_filter.setFilterLimits(this->intensity_filter_min_intensity, this->intensity_filter_max_intensity);
   point_cloud_ptr pc_without_i = boost::make_shared<point_cloud>();
   point_cloud_i_ptr pc_i_filtered = boost::make_shared<point_cloud_i>();
-  _temp_intensity_filter.filter(*pc_i_filtered);
+  _intensity_filter.filter(*pc_i_filtered);
 
   pc_without_i->points.resize(pc_i_filtered->size());
   for (size_t i = 0; i < pc_i_filtered->points.size(); i++)