diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar.yaml index cdde70798..f4ec88287 100644 --- 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 index 8c50bb395..39c0d39fa 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml @@ -12,8 +12,8 @@ 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 @@ -21,6 +21,10 @@ 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 index c0c5463ac..658a0bf15 100755 --- 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/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp index 333b82b79..1db53eaa9 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_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 index 0c1ae8e7e..5a0ccf137 100644 --- 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()) { 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 _temp_intensity_filter; - _temp_intensity_filter.setInputCloud(pc); - _temp_intensity_filter.setFilterFieldName("intensity"); - _temp_intensity_filter.setFilterLimits(10, 100); + // Intensity filter + pcl::PassThrough _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_i_ptr pc_i_filtered = boost::make_shared(); - _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++)