diff --git a/include/dual_laser_merger/dual_laser_merger.hpp b/include/dual_laser_merger/dual_laser_merger.hpp index f513aaa..3605154 100644 --- a/include/dual_laser_merger/dual_laser_merger.hpp +++ b/include/dual_laser_merger/dual_laser_merger.hpp @@ -17,12 +17,14 @@ #include #include +#include #include #include #include #include #include +#include #include "laser_geometry/laser_geometry.hpp" #include "message_filters/subscriber.h" @@ -56,9 +58,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 +79,17 @@ 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..13a6c77 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,40 @@ 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 +214,29 @@ 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 +284,7 @@ void MergerNode::sub_callback( } } - merged_pub->publish(merged); + merged_scan_pub->publish(merged); } }