From 5bed0f531f7651e81505bd95c9b0aeb8527bacba Mon Sep 17 00:00:00 2001 From: pradyum Date: Tue, 12 Nov 2024 07:58:54 +0530 Subject: [PATCH 1/4] added shadow and average filters (cherry picked from commit 208df64fa64d1b354712d9d8955f9d1ba9af399f) # Conflicts: # src/dual_laser_merger.cpp --- .../dual_laser_merger/dual_laser_merger.hpp | 17 ++++- launch/demo_laser_merger.launch.py | 3 + src/dual_laser_merger.cpp | 75 ++++++++++++++++++- 3 files changed, 87 insertions(+), 8 deletions(-) diff --git a/include/dual_laser_merger/dual_laser_merger.hpp b/include/dual_laser_merger/dual_laser_merger.hpp index f513aaa..29ce8f5 100644 --- a/include/dual_laser_merger/dual_laser_merger.hpp +++ b/include/dual_laser_merger/dual_laser_merger.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -56,9 +57,12 @@ class MergerNode : public rclcpp::Node message_filter; message_filters::Subscriber laser_1_sub; message_filters::Subscriber laser_2_sub; - rclcpp::Publisher::SharedPtr merged_pub; + rclcpp::Publisher::SharedPtr merged_scan_pub; + rclcpp::Publisher::SharedPtr merged_cloud_pub; laser_geometry::LaserProjection projector; + sensor_msgs::msg::LaserScan lidar_1_avg; + sensor_msgs::msg::LaserScan lidar_2_avg; sensor_msgs::msg::LaserScan merged; sensor_msgs::msg::PointCloud2 cloud_in_1; sensor_msgs::msg::PointCloud2 cloud_in_2; @@ -74,11 +78,16 @@ class MergerNode : public rclcpp::Node double tolerance_param, min_height_param, max_height_param, angle_min_param, angle_max_param, angle_increment_param, scan_time_param, range_min_param, range_max_param, inf_epsilon_param, laser_1_x_offset, laser_1_y_offset, laser_1_yaw_offset, laser_2_x_offset, laser_2_y_offset, - laser_2_yaw_offset; - bool use_inf_param, enable_calibration_param; + laser_2_yaw_offset, allowed_radius_param; + bool use_inf_param, enable_calibration_param, enable_shadow_filter_param, enable_average_filter_param; uint32_t ranges_size; double range, angle; - int index; + int index, numNearbyPoints; + double allowed_radius_scaled, dist_from_origin; + + pcl::KdTreeFLANN kdtree; + std::vector pointIndices; + std::vector pointDistances; void sub_callback( const sensor_msgs::msg::LaserScan::ConstSharedPtr & lidar_1_msg, diff --git a/launch/demo_laser_merger.launch.py b/launch/demo_laser_merger.launch.py index fe35927..445f3a6 100644 --- a/launch/demo_laser_merger.launch.py +++ b/launch/demo_laser_merger.launch.py @@ -66,6 +66,9 @@ def generate_launch_description(): {'angle_max': 3.141592654}, {'inf_epsilon': 1.0}, {'use_inf': True}, + {'allowed_radius': 0.45}, + {'enable_shadow_filter': True}, + {'enable_average_filter': True}, ], ) ], diff --git a/src/dual_laser_merger.cpp b/src/dual_laser_merger.cpp index 52f772d..fee438d 100644 --- a/src/dual_laser_merger.cpp +++ b/src/dual_laser_merger.cpp @@ -29,6 +29,7 @@ MergerNode::MergerNode(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "Target Frame: %s", target_frame_param.c_str()); } +<<<<<<< HEAD merged_pub = this->create_publisher( this->get_parameter( @@ -39,6 +40,18 @@ MergerNode::MergerNode(const rclcpp::NodeOptions & options) laser_2_sub.subscribe( this, this->get_parameter("laser_2_topic").as_string(), rclcpp::SensorDataQoS().get_rmw_qos_profile()); +======= + merged_scan_pub = + this->create_publisher(this->get_parameter( + "merged_scan_topic").as_string(), rclcpp::SensorDataQoS()); + merged_cloud_pub = + this->create_publisher(this->get_parameter( + "merged_cloud_topic").as_string(), rclcpp::SensorDataQoS()); + laser_1_sub.subscribe(this, this->get_parameter("laser_1_topic").as_string(), + rclcpp::SensorDataQoS()); + laser_2_sub.subscribe(this, this->get_parameter("laser_2_topic").as_string(), + rclcpp::SensorDataQoS()); +>>>>>>> 208df64 (added shadow and average filters) tf2_buffer = std::make_shared(this->get_clock()); tf2_listener = std::make_shared(*tf2_buffer, this); @@ -58,7 +71,8 @@ void MergerNode::declare_param() { this->declare_parameter("laser_1_topic", "laser_1"); this->declare_parameter("laser_2_topic", "laser_2"); - this->declare_parameter("merged_topic", "merged"); + this->declare_parameter("merged_scan_topic", "merged"); + this->declare_parameter("merged_cloud_topic", "merged_cloud"); target_frame_param = this->declare_parameter("target_frame", ""); tolerance_param = this->declare_parameter("tolerance", 0.01); input_queue_size_param = @@ -80,6 +94,9 @@ void MergerNode::declare_param() laser_2_x_offset = this->declare_parameter("laser_2_x_offset", 0.0); laser_2_y_offset = this->declare_parameter("laser_2_y_offset", 0.0); laser_2_yaw_offset = this->declare_parameter("laser_2_yaw_offset", 0.0); + allowed_radius_param = this->declare_parameter("allowed_radius", 1.0); + enable_shadow_filter_param = this->declare_parameter("enable_shadow_filter", false); + enable_average_filter_param = this->declare_parameter("enable_average_filter", false); } void MergerNode::refresh_param() @@ -102,6 +119,9 @@ void MergerNode::refresh_param() this->get_parameter("laser_2_x_offset", laser_2_x_offset); this->get_parameter("laser_2_y_offset", laser_2_y_offset); this->get_parameter("laser_2_yaw_offset", laser_2_yaw_offset); + this->get_parameter("allowed_radius", allowed_radius_param); + this->get_parameter("enable_shadow_filter", enable_shadow_filter_param); + this->get_parameter("enable_average_filter", enable_average_filter_param); } void MergerNode::sub_callback( @@ -116,8 +136,34 @@ void MergerNode::sub_callback( refresh_param(); } - projector.projectLaser(*lidar_1_msg, cloud_in_1); - projector.projectLaser(*lidar_2_msg, cloud_in_2); + if(enable_average_filter_param) { + lidar_1_avg = *lidar_1_msg; + lidar_2_avg = *lidar_2_msg; + for(size_t i = 0; i <= lidar_1_msg->ranges.size(); i++) { + if(i==0) { + lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[lidar_1_msg->ranges.size()-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i+1])/3; + } else if(i==(lidar_1_msg->ranges.size()-1)) { + lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[0])/3; + } else { + lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i+1])/3; + } + } + for(size_t i = 0; i <= lidar_2_msg->ranges.size(); i++) { + if(i==0) { + lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[lidar_2_msg->ranges.size()-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i+1])/3; + } else if(i==(lidar_2_msg->ranges.size()-1)) { + lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[0])/3; + } else { + lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i+1])/3; + } + } + + projector.projectLaser(lidar_1_avg, cloud_in_1); + projector.projectLaser(lidar_2_avg, cloud_in_2); + } else { + projector.projectLaser(*lidar_1_msg, cloud_in_1); + projector.projectLaser(*lidar_2_msg, cloud_in_2); + } if (lidar_1_msg->header.frame_id != target_frame_param) { tf2_msg.header = cloud_in_1.header; @@ -175,7 +221,28 @@ void MergerNode::sub_callback( pcl_cloud_out = pcl_cloud_in_1; pcl_cloud_out += pcl_cloud_in_2; + if(enable_shadow_filter_param) { + allowed_radius_scaled = allowed_radius_param / range_max_param; + kdtree.setInputCloud(pcl_cloud_out.makeShared()); + + for (auto& point : pcl_cloud_out.points) { + dist_from_origin = std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2)); + numNearbyPoints = kdtree.radiusSearch(point, allowed_radius_scaled * dist_from_origin, pointIndices, pointDistances); + numNearbyPoints -= 1; + if(numNearbyPoints == 0) { + if(use_inf_param) { + point.x = std::numeric_limits::infinity(); + point.y = std::numeric_limits::infinity(); + } else { + point.x = range_max_param + inf_epsilon_param; + point.y = range_max_param + inf_epsilon_param; + } + } + } + } + pcl::toROSMsg(pcl_cloud_out, cloud_out); + merged_cloud_pub->publish(cloud_out); merged.header = cloud_out.header; merged.header.frame_id = target_frame_param; @@ -223,7 +290,7 @@ void MergerNode::sub_callback( } } - merged_pub->publish(merged); + merged_scan_pub->publish(merged); } } From eac57241984cc3f0247616a3d2e93e9e7fef3815 Mon Sep 17 00:00:00 2001 From: pradyum Date: Tue, 12 Nov 2024 08:07:02 +0530 Subject: [PATCH 2/4] added vector header and linted (cherry picked from commit 831b426af94c6b7ce2f005106a8e194aac9f009c) --- .../dual_laser_merger/dual_laser_merger.hpp | 4 ++- src/dual_laser_merger.cpp | 31 ++++++++++++------- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/include/dual_laser_merger/dual_laser_merger.hpp b/include/dual_laser_merger/dual_laser_merger.hpp index 29ce8f5..3605154 100644 --- a/include/dual_laser_merger/dual_laser_merger.hpp +++ b/include/dual_laser_merger/dual_laser_merger.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include "laser_geometry/laser_geometry.hpp" #include "message_filters/subscriber.h" @@ -79,7 +80,8 @@ class MergerNode : public rclcpp::Node angle_increment_param, scan_time_param, range_min_param, range_max_param, inf_epsilon_param, laser_1_x_offset, laser_1_y_offset, laser_1_yaw_offset, laser_2_x_offset, laser_2_y_offset, laser_2_yaw_offset, allowed_radius_param; - bool use_inf_param, enable_calibration_param, enable_shadow_filter_param, enable_average_filter_param; + bool use_inf_param, enable_calibration_param, enable_shadow_filter_param, + enable_average_filter_param; uint32_t ranges_size; double range, angle; int index, numNearbyPoints; diff --git a/src/dual_laser_merger.cpp b/src/dual_laser_merger.cpp index fee438d..9a213a9 100644 --- a/src/dual_laser_merger.cpp +++ b/src/dual_laser_merger.cpp @@ -140,21 +140,27 @@ void MergerNode::sub_callback( lidar_1_avg = *lidar_1_msg; lidar_2_avg = *lidar_2_msg; for(size_t i = 0; i <= lidar_1_msg->ranges.size(); i++) { - if(i==0) { - lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[lidar_1_msg->ranges.size()-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i+1])/3; - } else if(i==(lidar_1_msg->ranges.size()-1)) { - lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[0])/3; + if(i == 0) { + lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[lidar_1_msg->ranges.size() - 1] + + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i + 1]) / 3; + } else if(i == (lidar_1_msg->ranges.size() - 1)) { + lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i - 1] + lidar_1_msg->ranges[i] + + lidar_1_msg->ranges[0]) / 3; } else { - lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i+1])/3; + lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i - 1] + lidar_1_msg->ranges[i] + + lidar_1_msg->ranges[i + 1]) / 3; } } for(size_t i = 0; i <= lidar_2_msg->ranges.size(); i++) { - if(i==0) { - lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[lidar_2_msg->ranges.size()-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i+1])/3; - } else if(i==(lidar_2_msg->ranges.size()-1)) { - lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[0])/3; + if(i == 0) { + lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[lidar_2_msg->ranges.size() - 1] + + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i + 1]) / 3; + } else if(i == (lidar_2_msg->ranges.size() - 1)) { + lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i - 1] + lidar_2_msg->ranges[i] + + lidar_2_msg->ranges[0]) / 3; } else { - lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i+1])/3; + lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i - 1] + lidar_2_msg->ranges[i] + + lidar_2_msg->ranges[i + 1]) / 3; } } @@ -225,9 +231,10 @@ void MergerNode::sub_callback( allowed_radius_scaled = allowed_radius_param / range_max_param; kdtree.setInputCloud(pcl_cloud_out.makeShared()); - for (auto& point : pcl_cloud_out.points) { + for (auto & point : pcl_cloud_out.points) { dist_from_origin = std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2)); - numNearbyPoints = kdtree.radiusSearch(point, allowed_radius_scaled * dist_from_origin, pointIndices, pointDistances); + numNearbyPoints = kdtree.radiusSearch(point, allowed_radius_scaled * dist_from_origin, + pointIndices, pointDistances); numNearbyPoints -= 1; if(numNearbyPoints == 0) { if(use_inf_param) { From 6c597d3310e4f2884705afe9b2793d30f45ee017 Mon Sep 17 00:00:00 2001 From: pradyum Date: Tue, 12 Nov 2024 08:16:52 +0530 Subject: [PATCH 3/4] fixed conflict --- src/dual_laser_merger.cpp | 23 ++++++----------------- 1 file changed, 6 insertions(+), 17 deletions(-) diff --git a/src/dual_laser_merger.cpp b/src/dual_laser_merger.cpp index 9a213a9..9361cdf 100644 --- a/src/dual_laser_merger.cpp +++ b/src/dual_laser_merger.cpp @@ -29,29 +29,18 @@ MergerNode::MergerNode(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "Target Frame: %s", target_frame_param.c_str()); } -<<<<<<< HEAD - merged_pub = - this->create_publisher( - this->get_parameter( - "merged_topic").as_string(), rclcpp::SensorDataQoS()); - laser_1_sub.subscribe( - this, this->get_parameter("laser_1_topic").as_string(), - rclcpp::SensorDataQoS().get_rmw_qos_profile()); - laser_2_sub.subscribe( - this, this->get_parameter("laser_2_topic").as_string(), - rclcpp::SensorDataQoS().get_rmw_qos_profile()); -======= merged_scan_pub = this->create_publisher(this->get_parameter( "merged_scan_topic").as_string(), rclcpp::SensorDataQoS()); merged_cloud_pub = this->create_publisher(this->get_parameter( "merged_cloud_topic").as_string(), rclcpp::SensorDataQoS()); - laser_1_sub.subscribe(this, this->get_parameter("laser_1_topic").as_string(), - rclcpp::SensorDataQoS()); - laser_2_sub.subscribe(this, this->get_parameter("laser_2_topic").as_string(), - rclcpp::SensorDataQoS()); ->>>>>>> 208df64 (added shadow and average filters) + laser_1_sub.subscribe( + this, this->get_parameter("laser_1_topic").as_string(), + rclcpp::SensorDataQoS().get_rmw_qos_profile()); + laser_2_sub.subscribe( + this, this->get_parameter("laser_2_topic").as_string(), + rclcpp::SensorDataQoS().get_rmw_qos_profile()); tf2_buffer = std::make_shared(this->get_clock()); tf2_listener = std::make_shared(*tf2_buffer, this); From 781cb2df048b1cc6c790c0e908601a4f94f58ec2 Mon Sep 17 00:00:00 2001 From: pradyum Date: Tue, 12 Nov 2024 08:22:30 +0530 Subject: [PATCH 4/4] removed humble lint --- CMakeLists.txt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a8129f0..7e93617 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,13 +21,13 @@ install(DIRECTORY bag DESTINATION share/dual_laser_merger) install(DIRECTORY config DESTINATION share/dual_laser_merger) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_flake8_FOUND TRUE) - set(ament_pep257_FOUND TRUE) - set(ament_xmllint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + #find_package(ament_lint_auto REQUIRED) + #set(ament_cmake_copyright_FOUND TRUE) + #set(ament_cmake_cpplint_FOUND TRUE) + #set(ament_flake8_FOUND TRUE) + #set(ament_pep257_FOUND TRUE) + #set(ament_xmllint_FOUND TRUE) + #ament_lint_auto_find_test_dependencies() endif() ament_auto_package() \ No newline at end of file