Skip to content

Commit

Permalink
added shadow and average filters
Browse files Browse the repository at this point in the history 
(cherry picked from commit 208df64)
  • Loading branch information
@pradyum @mergify
pradyum authored and mergify[bot] committed Nov 12, 2024
1 parent 2797e03 commit 66f8ac0
Show file tree
Hide file tree
Showing 3 changed files with 79 additions and 10 deletions.
17 changes: 13 additions & 4 deletions include/dual_laser_merger/dual_laser_merger.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>

#include <chrono>
Expand Down Expand Up @@ -56,9 +57,12 @@ class MergerNode : public rclcpp::Node
message_filter;
message_filters::Subscriber<sensor_msgs::msg::LaserScan> laser_1_sub;
message_filters::Subscriber<sensor_msgs::msg::LaserScan> laser_2_sub;
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr merged_pub;
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr merged_scan_pub;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::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;
Expand All @@ -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<pcl::PointXYZ> kdtree;
std::vector<int> pointIndices;
std::vector<float> pointDistances;

void sub_callback(
const sensor_msgs::msg::LaserScan::ConstSharedPtr & lidar_1_msg,
Expand Down
3 changes: 3 additions & 0 deletions launch/demo_laser_merger.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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},
],
)
],
Expand Down
69 changes: 63 additions & 6 deletions src/dual_laser_merger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<sensor_msgs::msg::LaserScan>(this->get_parameter(
"merged_topic").as_string(), rclcpp::SensorDataQoS());
"merged_scan_topic").as_string(), rclcpp::SensorDataQoS());
merged_cloud_pub =
this->create_publisher<sensor_msgs::msg::PointCloud2>(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().get_rmw_qos_profile());
laser_2_sub.subscribe(this, this->get_parameter("laser_2_topic").as_string(),
Expand All @@ -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 =
Expand All @@ -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()
Expand All @@ -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(
Expand All @@ -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;
Expand Down Expand Up @@ -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<double>::infinity();
point.y = std::numeric_limits<double>::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;
Expand Down Expand Up @@ -220,7 +277,7 @@ void MergerNode::sub_callback(
}
}

merged_pub->publish(merged);
merged_scan_pub->publish(merged);
}
}

Expand Down

0 comments on commit 66f8ac0

Please sign in to comment.