From 208df64fa64d1b354712d9d8955f9d1ba9af399f Mon Sep 17 00:00:00 2001 From: pradyum Date: Tue, 12 Nov 2024 07:58:54 +0530 Subject: [PATCH 1/2] added shadow and average filters --- .../dual_laser_merger/dual_laser_merger.hpp | 17 +++-- launch/demo_laser_merger.launch.py | 3 + src/dual_laser_merger.cpp | 69 +++++++++++++++++-- 3 files changed, 79 insertions(+), 10 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 485ac00..15de49f 100644 --- a/src/dual_laser_merger.cpp +++ b/src/dual_laser_merger.cpp @@ -29,9 +29,12 @@ MergerNode::MergerNode(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "Target Frame: %s", target_frame_param.c_str()); } - merged_pub = + merged_scan_pub = this->create_publisher(this->get_parameter( - "merged_topic").as_string(), rclcpp::SensorDataQoS()); + "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(), @@ -55,7 +58,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 = @@ -77,6 +81,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() @@ -99,6 +106,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( @@ -113,8 +123,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; @@ -172,7 +208,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; @@ -220,7 +277,7 @@ void MergerNode::sub_callback( } } - merged_pub->publish(merged); + merged_scan_pub->publish(merged); } } From 831b426af94c6b7ce2f005106a8e194aac9f009c Mon Sep 17 00:00:00 2001 From: pradyum Date: Tue, 12 Nov 2024 08:07:02 +0530 Subject: [PATCH 2/2] added vector header and linted --- .../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 15de49f..13a6c77 100644 --- a/src/dual_laser_merger.cpp +++ b/src/dual_laser_merger.cpp @@ -127,21 +127,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; } } @@ -212,9 +218,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) {