Skip to content

Commit

Permalink
Merge pull request #33 from pradyum/mergify/bp/humble/pr-31
Browse files Browse the repository at this point in the history 
added shadow and average filters (backport #31)
  • Loading branch information
@pradyum
pradyum authored Nov 12, 2024
2 parents 635a461 + 781cb2d commit 334fa7b
Show file tree
Hide file tree
Showing 4 changed files with 96 additions and 19 deletions.
14 changes: 7 additions & 7 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
19 changes: 15 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,12 +17,14 @@

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

#include <chrono>
#include <cmath>
#include <memory>
#include <string>
#include <vector>

#include "laser_geometry/laser_geometry.hpp"
#include "message_filters/subscriber.h"
Expand Down Expand Up @@ -56,9 +58,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 +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<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
79 changes: 71 additions & 8 deletions src/dual_laser_merger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,10 +29,12 @@ MergerNode::MergerNode(const rclcpp::NodeOptions & options)
RCLCPP_INFO(this->get_logger(), "Target Frame: %s", target_frame_param.c_str());
}

merged_pub =
this->create_publisher<sensor_msgs::msg::LaserScan>(
this->get_parameter(
"merged_topic").as_string(), rclcpp::SensorDataQoS());
merged_scan_pub =
this->create_publisher<sensor_msgs::msg::LaserScan>(this->get_parameter(
"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());
Expand All @@ -58,7 +60,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 @@ -80,6 +83,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 @@ -102,6 +108,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 @@ -116,8 +125,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;
Expand Down Expand Up @@ -175,7 +216,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<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 @@ -223,7 +286,7 @@ void MergerNode::sub_callback(
}
}

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

Expand Down

0 comments on commit 334fa7b

Please sign in to comment.