diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fa17f0..f6e53fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,10 +13,16 @@ ament_auto_find_build_dependencies() ############################################################################## ament_auto_add_library(laser_scan_filters SHARED src/laser_scan_filters.cpp) +ament_auto_add_library(laser_filter_chains SHARED + src/scan_to_cloud_filter_chain.cpp + src/scan_to_scan_filter_chain.cpp) +rclcpp_components_register_nodes(laser_filter_chains + "laser_filters::ScanToCloudFilterChain" + "laser_filters::ScanToScanFilterChain") set(FILTER_CHAINS - scan_to_cloud_filter_chain - scan_to_scan_filter_chain + scan_to_cloud_filter_chain_node + scan_to_scan_filter_chain_node generic_laser_filter_node ) foreach(FILTER_CHAIN ${FILTER_CHAINS}) diff --git a/examples/angular_filter_example.launch.py b/examples/angular_filter_example.launch.py index ff42e06..3ea7f76 100644 --- a/examples/angular_filter_example.launch.py +++ b/examples/angular_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/box_filter_example.launch.py b/examples/box_filter_example.launch.py index 5cd7e32..65fec36 100644 --- a/examples/box_filter_example.launch.py +++ b/examples/box_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/footprint_filter_example.launch.py b/examples/footprint_filter_example.launch.py index e868786..28948fb 100644 --- a/examples/footprint_filter_example.launch.py +++ b/examples/footprint_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/intensity_filter_example.launch.py b/examples/intensity_filter_example.launch.py index 6d01279..0fdc669 100644 --- a/examples/intensity_filter_example.launch.py +++ b/examples/intensity_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/mask_filter_example.launch.py b/examples/mask_filter_example.launch.py index 54ab6d7..8797b82 100644 --- a/examples/mask_filter_example.launch.py +++ b/examples/mask_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/median_filter_example.launch.py b/examples/median_filter_example.launch.py index 13db13a..a42d133 100644 --- a/examples/median_filter_example.launch.py +++ b/examples/median_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/multiple_filters_example.launch.py b/examples/multiple_filters_example.launch.py index 9c704a2..a28fed6 100644 --- a/examples/multiple_filters_example.launch.py +++ b/examples/multiple_filters_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/pass_through_example.launch.py b/examples/pass_through_example.launch.py index baa7ba2..ff6b97a 100644 --- a/examples/pass_through_example.launch.py +++ b/examples/pass_through_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/range_filter_example.launch.py b/examples/range_filter_example.launch.py index fec7d46..934a0c9 100644 --- a/examples/range_filter_example.launch.py +++ b/examples/range_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/examples/shadow_filter_example.launch.py b/examples/shadow_filter_example.launch.py index 6ccfafe..f084acf 100644 --- a/examples/shadow_filter_example.launch.py +++ b/examples/shadow_filter_example.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package="laser_filters", - executable="scan_to_scan_filter_chain", + executable="scan_to_scan_filter_chain_node", parameters=[ PathJoinSubstitution([ get_package_share_directory("laser_filters"), diff --git a/package.xml b/package.xml index 6983876..6e095fc 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ pluginlib rclcpp rclcpp_lifecycle + rclcpp_components sensor_msgs tf2 tf2_ros diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index e3e416d..a618fd4 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -34,173 +34,122 @@ */ -#include -#include -#include +#include +#include "scan_to_cloud_filter_chain.hpp" -// TF -#include -#include "tf2_ros/message_filter.h" -#include "tf2_ros/create_timer_ros.h" +namespace laser_filters +{ +ScanToCloudFilterChain::ScanToCloudFilterChain( + const rclcpp::NodeOptions & options, + const std::string & ns) +: rclcpp::Node("scan_to_cloud_filter_chain", ns, options), + laser_max_range_(DBL_MAX), + buffer_(this->get_clock()), + tf_(buffer_), + sub_(this, "scan", rmw_qos_profile_sensor_data), + filter_(sub_, buffer_, "", 50, this->shared_from_this()), + cloud_filter_chain_("sensor_msgs::msg::PointCloud2"), + scan_filter_chain_("sensor_msgs::msg::LaserScan") +{ + this->declare_parameter("high_fidelity", false); + this->declare_parameter("notifier_tolerance", 0.03); + this->declare_parameter("target_frame", std::string("base_link")); + this->declare_parameter("incident_angle_correction", true); -#include "message_filters/subscriber.h" + this->get_parameter("high_fidelity", high_fidelity_); + this->get_parameter("notifier_tolerance", tf_tolerance_); + this->get_parameter("target_frame", target_frame_); + this->get_parameter("incident_angle_correction", incident_angle_correction_); -#include + this->get_parameter_or("filter_window", window_, 2); + this->get_parameter_or("laser_max_range", laser_max_range_, DBL_MAX); + this->get_parameter_or("scan_topic", scan_topic_, std::string("tilt_scan")); + this->get_parameter_or("cloud_topic", cloud_topic_, std::string("tilt_laser_cloud_filtered")); -// Laser projection -#include -//Filters -#include "filters/filter_chain.hpp" + filter_.setTargetFrame(target_frame_); + filter_.registerCallback( + std::bind( + &ScanToCloudFilterChain::scanCallback, this, + std::placeholders::_1)); + filter_.setTolerance(std::chrono::duration(tf_tolerance_)); -/** @b ScanShadowsFilter is a simple node that filters shadow points in a laser scan line and publishes the results in a cloud. - */ -class ScanToCloudFilterChain -{ -public: - - // ROS related - laser_geometry::LaserProjection projector_; // Used to project laser scans - - rclcpp::Node::SharedPtr nh_; - double laser_max_range_; // Used in laser scan projection - int window_; - - bool high_fidelity_; // High fidelity (interpolating time across scan) - std::string target_frame_; // Target frame for high fidelity result - std::string scan_topic_, cloud_topic_; - - // TF - tf2_ros::Buffer buffer_; - tf2_ros::TransformListener tf_; - - message_filters::Subscriber sub_; - tf2_ros::MessageFilter filter_; - - double tf_tolerance_; - filters::FilterChain cloud_filter_chain_; - filters::FilterChain scan_filter_chain_; - rclcpp::Publisher::SharedPtr cloud_pub_; - - bool incident_angle_correction_; - - //////////////////////////////////////////////////////////////////////////////// - ScanToCloudFilterChain(rclcpp::Node::SharedPtr node) : nh_(node), - laser_max_range_(DBL_MAX), - buffer_(nh_->get_clock()), - tf_(buffer_), - sub_(nh_, "scan", rmw_qos_profile_sensor_data), - filter_(sub_, buffer_, "", 50, nh_), - cloud_filter_chain_("sensor_msgs::msg::PointCloud2"), - scan_filter_chain_("sensor_msgs::msg::LaserScan") - { - nh_->declare_parameter("high_fidelity", false); - nh_->declare_parameter("notifier_tolerance", 0.03); - nh_->declare_parameter("target_frame", std::string("base_link")); - nh_->declare_parameter("incident_angle_correction", true); - - nh_->get_parameter("high_fidelity", high_fidelity_); - nh_->get_parameter("notifier_tolerance", tf_tolerance_); - nh_->get_parameter("target_frame", target_frame_); - nh_->get_parameter("incident_angle_correction", incident_angle_correction_); - - nh_->get_parameter_or("filter_window", window_, 2); - nh_->get_parameter_or("laser_max_range", laser_max_range_, DBL_MAX); - nh_->get_parameter_or("scan_topic", scan_topic_, std::string("tilt_scan")); - nh_->get_parameter_or("cloud_topic", cloud_topic_, std::string("tilt_laser_cloud_filtered")); - - - filter_.setTargetFrame(target_frame_); - filter_.registerCallback(std::bind(&ScanToCloudFilterChain::scanCallback, this, std::placeholders::_1)); - filter_.setTolerance(std::chrono::duration(tf_tolerance_)); - - auto timer_interface = std::make_shared( - nh_->get_node_base_interface(), - nh_->get_node_timers_interface()); - buffer_.setCreateTimerInterface(timer_interface); - - sub_.subscribe(nh_, "scan", rmw_qos_profile_sensor_data); - - filter_.connectInput(sub_); - - cloud_pub_ = nh_->create_publisher("cloud_filtered", 10); - - cloud_filter_chain_.configure("cloud_filter_chain", nh_->get_node_logging_interface(), nh_->get_node_parameters_interface()); - - scan_filter_chain_.configure("scan_filter_chain", nh_->get_node_logging_interface(), nh_->get_node_parameters_interface()); - } + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + buffer_.setCreateTimerInterface(timer_interface); - //////////////////////////////////////////////////////////////////////////////// - void - scanCallback (const std::shared_ptr& scan_msg) - { - // sensor_msgs::msg::LaserScan scan_msg = *scan_in; - - sensor_msgs::msg::LaserScan filtered_scan; - scan_filter_chain_.update (*scan_msg, filtered_scan); - - // Project laser into point cloud - sensor_msgs::msg::PointCloud2 scan_cloud; - - //\TODO CLEAN UP HACK - // This is a trial at correcting for incident angles. It makes many assumptions that do not generalise - if(incident_angle_correction_) - { - for (unsigned int i = 0; i < filtered_scan.ranges.size(); i++) - { - double angle = filtered_scan.angle_min + i * filtered_scan.angle_increment; - filtered_scan.ranges[i] = filtered_scan.ranges[i] + 0.03 * exp(-fabs(sin(angle))); - } - } + sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); - // Transform into a PointCloud message - int mask = laser_geometry::channel_option::Intensity | - laser_geometry::channel_option::Distance | - laser_geometry::channel_option::Index | - laser_geometry::channel_option::Timestamp; - - if (high_fidelity_) - { - try - { - projector_.transformLaserScanToPointCloud(target_frame_, filtered_scan, scan_cloud, buffer_, mask); - } - catch (tf2::TransformException &ex) - { - RCLCPP_WARN(nh_->get_logger(), "High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str(), ex.what()); - return; - //projector_.projectLaser (filtered_scan, scan_cloud, laser_max_range_, preservative_, mask); - } - } - else - { - projector_.transformLaserScanToPointCloud(target_frame_, filtered_scan, scan_cloud, buffer_, laser_max_range_, mask); - } - - sensor_msgs::msg::PointCloud2 filtered_cloud; - cloud_filter_chain_.update (scan_cloud, filtered_cloud); + filter_.connectInput(sub_); - cloud_pub_->publish(filtered_cloud); - } - -} ; + cloud_pub_ = this->create_publisher("cloud_filtered", 10); + cloud_filter_chain_.configure( + "cloud_filter_chain", + this->get_node_logging_interface(), this->get_node_parameters_interface()); + scan_filter_chain_.configure( + "scan_filter_chain", + this->get_node_logging_interface(), this->get_node_parameters_interface()); +} -int -main (int argc, char** argv) +void +ScanToCloudFilterChain::scanCallback( + const std::shared_ptr & scan_msg) { - rclcpp::init(argc, argv); - auto nh = rclcpp::Node::make_shared("scan_to_cloud_filter_chain"); - ScanToCloudFilterChain f(nh); + // sensor_msgs::msg::LaserScan scan_msg = *scan_in; + + sensor_msgs::msg::LaserScan filtered_scan; + scan_filter_chain_.update(*scan_msg, filtered_scan); - rclcpp::WallRate loop_rate(200); - while (rclcpp::ok()) { + // Project laser into point cloud + sensor_msgs::msg::PointCloud2 scan_cloud; - rclcpp::spin_some(nh); - loop_rate.sleep(); + //\TODO CLEAN UP HACK + // This is a trial at correcting for incident angles. It makes many assumptions that do not generalise + if (incident_angle_correction_) { + for (unsigned int i = 0; i < filtered_scan.ranges.size(); i++) { + double angle = filtered_scan.angle_min + i * filtered_scan.angle_increment; + filtered_scan.ranges[i] = filtered_scan.ranges[i] + 0.03 * exp(-fabs(sin(angle))); + } + } + // Transform into a PointCloud message + int mask = laser_geometry::channel_option::Intensity | + laser_geometry::channel_option::Distance | + laser_geometry::channel_option::Index | + laser_geometry::channel_option::Timestamp; + + if (high_fidelity_) { + try { + projector_.transformLaserScanToPointCloud( + target_frame_, filtered_scan, scan_cloud, buffer_, + mask); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + this->get_logger(), "High fidelity enabled, but TF returned a transform exception to frame %s: %s", + target_frame_.c_str(), ex.what()); + return; + //projector_.projectLaser (filtered_scan, scan_cloud, laser_max_range_, preservative_, mask); + } + } else { + projector_.transformLaserScanToPointCloud( + target_frame_, filtered_scan, scan_cloud, buffer_, + laser_max_range_, mask); } - return (0); + sensor_msgs::msg::PointCloud2 filtered_cloud; + cloud_filter_chain_.update(scan_cloud, filtered_cloud); + + cloud_pub_->publish(filtered_cloud); } +} // namespace laser_filters + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(laser_filters::ScanToCloudFilterChain) diff --git a/src/scan_to_cloud_filter_chain.hpp b/src/scan_to_cloud_filter_chain.hpp new file mode 100644 index 0000000..05f5465 --- /dev/null +++ b/src/scan_to_cloud_filter_chain.hpp @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2008 Radu Bogdan Rusu + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $ + * + */ + +/* +\author Radu Bogdan Rusu + + + */ + +#include +#include +#include + +// TF +#include +#include "tf2_ros/message_filter.h" +#include "tf2_ros/create_timer_ros.h" + +#include "message_filters/subscriber.h" + +// Laser projection +#include + +//Filters +#include "filters/filter_chain.hpp" + +namespace laser_filters +{ +class ScanToCloudFilterChain : public rclcpp::Node +{ +public: + // ROS related + laser_geometry::LaserProjection projector_; // Used to project laser scans + + double laser_max_range_; // Used in laser scan projection + int window_; + + bool high_fidelity_; // High fidelity (interpolating time across scan) + std::string target_frame_; // Target frame for high fidelity result + std::string scan_topic_, cloud_topic_; + + // TF + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener tf_; + + message_filters::Subscriber sub_; + tf2_ros::MessageFilter filter_; + + double tf_tolerance_; + filters::FilterChain cloud_filter_chain_; + filters::FilterChain scan_filter_chain_; + rclcpp::Publisher::SharedPtr cloud_pub_; + + bool incident_angle_correction_; + + ScanToCloudFilterChain( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + const std::string & ns = ""); + + void + scanCallback(const std::shared_ptr & scan_msg); +}; +} // namespace laser_filters diff --git a/src/scan_to_cloud_filter_chain_node.cpp b/src/scan_to_cloud_filter_chain_node.cpp new file mode 100644 index 0000000..3b6c737 --- /dev/null +++ b/src/scan_to_cloud_filter_chain_node.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2008 Radu Bogdan Rusu + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $ + * + */ + +/* +\author Radu Bogdan Rusu + + + */ + + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "scan_to_cloud_filter_chain.hpp" + +int +main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto filter_chain = std::make_shared(); + + rclcpp::WallRate loop_rate(200); + while (rclcpp::ok()) { + + rclcpp::spin_some(filter_chain->get_node_base_interface()); + loop_rate.sleep(); + + } + + return 0; +} diff --git a/src/scan_to_scan_filter_chain.cpp b/src/scan_to_scan_filter_chain.cpp index 721b2d7..f2ed27c 100644 --- a/src/scan_to_scan_filter_chain.cpp +++ b/src/scan_to_scan_filter_chain.cpp @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright @@ -13,7 +13,7 @@ * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -28,109 +28,82 @@ */ -#include -#include +#include "scan_to_scan_filter_chain.hpp" -// TF -#include -#include "tf2_ros/message_filter.h" - -#include "message_filters/subscriber.h" - -#include "filters/filter_chain.hpp" - -class ScanToScanFilterChain +namespace laser_filters { -protected: - // Our NodeHandle - rclcpp::Node::SharedPtr nh_; - - // Components for tf::MessageFilter - std::shared_ptr tf_; - tf2_ros::Buffer buffer_; - - message_filters::Subscriber scan_sub_; - std::shared_ptr> tf_filter_; - double tf_filter_tolerance_; - - // Filter Chain - filters::FilterChain filter_chain_; - - // Components for publishing - sensor_msgs::msg::LaserScan msg_; - rclcpp::Publisher::SharedPtr output_pub_; - -public: - // Constructor - ScanToScanFilterChain(rclcpp::Node::SharedPtr node) - : nh_(node), - tf_(NULL), - buffer_(nh_->get_clock()), - scan_sub_(nh_, "scan", rmw_qos_profile_sensor_data), - tf_filter_(NULL), - filter_chain_("sensor_msgs::msg::LaserScan") - { - // Configure filter chain - filter_chain_.configure("", nh_->get_node_logging_interface(), nh_->get_node_parameters_interface()); - - std::string tf_message_filter_target_frame; - if (nh_->get_parameter("tf_message_filter_target_frame", tf_message_filter_target_frame)) - { - - nh_->get_parameter_or("tf_message_filter_tolerance", tf_filter_tolerance_, 0.03); - - tf_.reset(new tf2_ros::TransformListener(buffer_)); - tf_filter_.reset(new tf2_ros::MessageFilter(scan_sub_, buffer_, "", 50, nh_)); - tf_filter_->setTargetFrame(tf_message_filter_target_frame); - tf_filter_->setTolerance(std::chrono::duration(tf_filter_tolerance_)); - - // Setup tf::MessageFilter generates callback - tf_filter_->registerCallback(std::bind(&ScanToScanFilterChain::callback, this, std::placeholders::_1)); - } - else - { - // Pass through if no tf_message_filter_target_frame - scan_sub_.registerCallback(std::bind(&ScanToScanFilterChain::callback, this, std::placeholders::_1)); - } - - // Advertise output - output_pub_ = nh_->create_publisher("scan_filtered", 1000); +// Constructor +ScanToScanFilterChain::ScanToScanFilterChain( + const rclcpp::NodeOptions & options, + const std::string & ns) +: rclcpp::Node("scan_to_scan_filter_chain", ns, options), + tf_(NULL), + buffer_(this->get_clock()), + scan_sub_(this, "scan", rmw_qos_profile_sensor_data), + tf_filter_(NULL), + filter_chain_("sensor_msgs::msg::LaserScan") +{ + // Configure filter chain + filter_chain_.configure( + "", + this->get_node_logging_interface(), this->get_node_parameters_interface()); + + std::string tf_message_filter_target_frame; + if (this->get_parameter("tf_message_filter_target_frame", tf_message_filter_target_frame)) { + + this->get_parameter_or("tf_message_filter_tolerance", tf_filter_tolerance_, 0.03); + + tf_.reset(new tf2_ros::TransformListener(buffer_)); + tf_filter_.reset( + new tf2_ros::MessageFilter( + scan_sub_, buffer_, "", + 50, this->shared_from_this())); + tf_filter_->setTargetFrame(tf_message_filter_target_frame); + tf_filter_->setTolerance(std::chrono::duration(tf_filter_tolerance_)); + + // Setup tf::MessageFilter generates callback + tf_filter_->registerCallback( + std::bind( + &ScanToScanFilterChain::callback, this, + std::placeholders::_1)); + } else { + // Pass through if no tf_message_filter_target_frame + scan_sub_.registerCallback( + std::bind( + &ScanToScanFilterChain::callback, this, + std::placeholders::_1)); } - // Destructor - ~ScanToScanFilterChain() - { - if (tf_filter_) - tf_filter_.reset(); - if (tf_) - tf_.reset(); - } + // Advertise output + output_pub_ = this->create_publisher("scan_filtered", 1000); +} - // Callback - void callback(const std::shared_ptr& msg_in) - { - // Run the filter chain - if (filter_chain_.update(*msg_in, msg_)) - { - //only publish result if filter succeeded - output_pub_->publish(msg_); - } +// Destructor +ScanToScanFilterChain::~ScanToScanFilterChain() +{ + if (tf_filter_) { + tf_filter_.reset(); + } + if (tf_) { + tf_.reset(); } -}; +} -int main(int argc, char **argv) +// Callback +void ScanToScanFilterChain::callback( + const std::shared_ptr & msg_in) { - rclcpp::init(argc, argv); - auto nh = rclcpp::Node::make_shared("scan_to_scan_filter_chain"); - ScanToScanFilterChain t(nh); - - rclcpp::WallRate loop_rate(200); - while (rclcpp::ok()) { - - rclcpp::spin_some(nh); - loop_rate.sleep(); - + // Run the filter chain + if (filter_chain_.update(*msg_in, msg_)) { + //only publish result if filter succeeded + output_pub_->publish(msg_); } - - return 0; } +} // namespace laser_filters + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(laser_filters::ScanToScanFilterChain) diff --git a/src/scan_to_scan_filter_chain.hpp b/src/scan_to_scan_filter_chain.hpp new file mode 100644 index 0000000..b111f4d --- /dev/null +++ b/src/scan_to_scan_filter_chain.hpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include + +// TF +#include +#include "tf2_ros/message_filter.h" + +#include "message_filters/subscriber.h" + +#include "filters/filter_chain.hpp" + +namespace laser_filters +{ +class ScanToScanFilterChain : public rclcpp::Node +{ +protected: + // Components for tf::MessageFilter + std::shared_ptr tf_; + tf2_ros::Buffer buffer_; + + message_filters::Subscriber scan_sub_; + std::shared_ptr> tf_filter_; + double tf_filter_tolerance_; + + // Filter Chain + filters::FilterChain filter_chain_; + + // Components for publishing + sensor_msgs::msg::LaserScan msg_; + rclcpp::Publisher::SharedPtr output_pub_; + +public: + // Constructor + ScanToScanFilterChain( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + const std::string & ns = ""); + + // Destructor + ~ScanToScanFilterChain(); + + // Callback + void callback(const std::shared_ptr & msg_in); +}; +} // namespace laser_filters diff --git a/src/scan_to_scan_filter_chain_node.cpp b/src/scan_to_scan_filter_chain_node.cpp new file mode 100644 index 0000000..555bbae --- /dev/null +++ b/src/scan_to_scan_filter_chain_node.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2008 Radu Bogdan Rusu + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $ + * + */ + +/* +\author Radu Bogdan Rusu + + + */ + + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "scan_to_scan_filter_chain.hpp" + +int +main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto filter_chain = std::make_shared(); + + rclcpp::WallRate loop_rate(200); + while (rclcpp::ok()) { + + rclcpp::spin_some(filter_chain->get_node_base_interface()); + loop_rate.sleep(); + + } + + return 0; +}