diff --git a/include/laser_filters/box_filter.h b/include/laser_filters/box_filter.h index 1a272ef7..5465b12b 100644 --- a/include/laser_filters/box_filter.h +++ b/include/laser_filters/box_filter.h @@ -9,11 +9,11 @@ * modification, are permitted provided that the following conditions * are met: * - * 1. Redistributions of source code must retain the above + * 1. Redistributions of source code must retain the above * copyright notice, this list of conditions and the following * disclaimer. * - * 2. Redistributions in binary form must reproduce the above + * 2. 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. @@ -32,7 +32,7 @@ * 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 + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * @@ -50,7 +50,7 @@ #include #include -#include +#include #include typedef tf2::Vector3 Point; @@ -168,12 +168,12 @@ class LaserScanBoxFilter : public filters::FilterBase iter_i(laser_cloud, "index"); sensor_msgs::PointCloud2ConstIterator iter_x(laser_cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(laser_cloud, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(laser_cloud, "z"); - + sensor_msgs::PointCloud2ConstIterator iter_z(laser_cloud, "z"); + if ( - !(iter_i != iter_i.end()) || - !(iter_x != iter_x.end()) || - !(iter_y != iter_y.end()) || + !(iter_i != iter_i.end()) || + !(iter_x != iter_x.end()) || + !(iter_y != iter_y.end()) || !(iter_z != iter_z.end())) { RCLCPP_INFO_THROTTLE(get_logger(), steady_clock, .3, "x, y, z and index fields are required, skipping scan"); diff --git a/include/laser_filters/footprint_filter.h b/include/laser_filters/footprint_filter.h index 81e704c0..33975908 100644 --- a/include/laser_filters/footprint_filter.h +++ b/include/laser_filters/footprint_filter.h @@ -44,8 +44,8 @@ This is useful for ground plane extraction #include "filters/filter_base.hpp" #include -#include -#include +#include +#include #include #include #include // PointCloud2ConstIterator @@ -61,7 +61,7 @@ class LaserScanFootprintFilter : public filters::FilterBase #include -#include +#include #include #include @@ -257,10 +257,10 @@ class LaserScanPolygonFilterBase : public filters::FilterBase("polygon", rclcpp::QoS(1).transient_local().keep_last(1)); is_polygon_published_ = false; - + return true; } @@ -288,7 +288,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBase(input_scan.ranges.size() * input_scan.time_increment), - 1.0s, + 1.0s, &error_msg ); if(!success){ @@ -490,7 +490,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { std::lock_guard lock(own_mutex_); publishPolygon(); - if (!is_polygon_transformed_) + if (!is_polygon_transformed_) { if (!transformPolygon(input_scan.header.frame_id)) return false; } @@ -525,7 +525,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { is_polygon_transformed_ = false; LaserScanPolygonFilterBase::footprintCB(polygon); } - + protected: bool transformPolygon(const std::string &input_scan_frame_id) { @@ -534,7 +534,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { "waitForTransform %s -> %s", polygon_frame_.c_str(), input_scan_frame_id.c_str() ); - + geometry_msgs::msg::TransformStamped transform; try { @@ -563,7 +563,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { is_polygon_transformed_ = true; return true; } - + rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector parameters) override { is_polygon_transformed_ = false; @@ -599,7 +599,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { } } } - geometry_msgs::msg::PointStamped createPointStamped(const double &x, + geometry_msgs::msg::PointStamped createPointStamped(const double &x, const double &y, const double &z, const builtin_interfaces::msg::Time &stamp, diff --git a/src/generic_laser_filter_node.cpp b/src/generic_laser_filter_node.cpp index b152aee1..2e8a7921 100644 --- a/src/generic_laser_filter_node.cpp +++ b/src/generic_laser_filter_node.cpp @@ -31,8 +31,8 @@ #include // TF -#include -#include "tf2_ros/message_filter.h" +#include +#include typedef tf2::TransformException TransformException; typedef tf2_ros::TransformListener TransformListener; @@ -78,7 +78,7 @@ class GenericLaserScanFilterNode tf_(buffer_), buffer_(nh_->get_clock()), scan_sub_(nh_, "scan", rclcpp::SensorDataQoS()), - tf_filter_(scan_sub_, buffer_, "base_link", 50, nh_), + tf_filter_(scan_sub_, buffer_, "base_link", 50, *nh_), filter_chain_("sensor_msgs::msg::LaserScan") { // Configure filter chain @@ -121,11 +121,12 @@ int main(int argc, char **argv) GenericLaserScanFilterNode t(nh); rclcpp::WallRate loop_rate(200); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(nh); while (rclcpp::ok()) { - rclcpp::spin_some(nh); + executor.spin_some(); loop_rate.sleep(); - } return 0; diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index b7674e65..859f7758 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -46,8 +46,7 @@ ScanToCloudFilterChain::ScanToCloudFilterChain( laser_max_range_(DBL_MAX), buffer_(this->get_clock()), tf_(buffer_), - filter_(scan_sub_, buffer_, "", 50, this->get_node_logging_interface(), - this->get_node_clock_interface()), + filter_(scan_sub_, buffer_, "", 50, *this), cloud_filter_chain_("sensor_msgs::msg::PointCloud2"), scan_filter_chain_("sensor_msgs::msg::LaserScan") { diff --git a/src/scan_to_cloud_filter_chain.hpp b/src/scan_to_cloud_filter_chain.hpp index ba8fa261..0e9b985d 100644 --- a/src/scan_to_cloud_filter_chain.hpp +++ b/src/scan_to_cloud_filter_chain.hpp @@ -41,11 +41,11 @@ #include // TF -#include -#include "tf2_ros/message_filter.h" -#include "tf2_ros/create_timer_ros.h" +#include +#include +#include -#include "message_filters/subscriber.hpp" +#include // Laser projection #include diff --git a/src/scan_to_scan_filter_chain.cpp b/src/scan_to_scan_filter_chain.cpp index 87173978..98743316 100644 --- a/src/scan_to_scan_filter_chain.cpp +++ b/src/scan_to_scan_filter_chain.cpp @@ -75,7 +75,7 @@ ScanToScanFilterChain::ScanToScanFilterChain( tf_filter_.reset( new tf2_ros::MessageFilter( scan_sub_, buffer_, "", - 50, this->get_node_logging_interface(), this->get_node_clock_interface())); + 50, *this)); tf_filter_->setTargetFrame(tf_message_filter_target_frame_); tf_filter_->setTolerance(std::chrono::duration(tf_filter_tolerance_)); diff --git a/src/scan_to_scan_filter_chain.hpp b/src/scan_to_scan_filter_chain.hpp index 58792a43..682545e7 100644 --- a/src/scan_to_scan_filter_chain.hpp +++ b/src/scan_to_scan_filter_chain.hpp @@ -34,10 +34,10 @@ #include // TF -#include -#include "tf2_ros/message_filter.h" +#include +#include -#include "message_filters/subscriber.hpp" +#include #include "filters/filter_chain.hpp"