Update code to work with rolling

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 <filters/filter_base.hpp>
 
 #include <tf2/transform_datatypes.hpp>
-#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_listener.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 
 typedef tf2::Vector3 Point;
@@ -168,12 +168,12 @@ class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::msg::LaserSca
       sensor_msgs::PointCloud2ConstIterator<int> iter_i(laser_cloud, "index");
       sensor_msgs::PointCloud2ConstIterator<float> iter_x(laser_cloud, "x");
       sensor_msgs::PointCloud2ConstIterator<float> iter_y(laser_cloud, "y");
-      sensor_msgs::PointCloud2ConstIterator<float> iter_z(laser_cloud, "z");      
-      
+      sensor_msgs::PointCloud2ConstIterator<float> 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");

include/laser_filters/footprint_filter.h

@@ -44,8 +44,8 @@ This is useful for ground plane extraction
 #include "filters/filter_base.hpp"
 
 #include <tf2/transform_datatypes.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.hpp>
+#include <tf2_ros/transform_listener.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp> // PointCloud2ConstIterator
@@ -61,7 +61,7 @@ class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::msg::La
 {
 public:
   LaserScanFootprintFilter()
-      : rclcpp_lifecycle::LifecycleNode("laser_scan_footprint_filter"), 
+      : rclcpp_lifecycle::LifecycleNode("laser_scan_footprint_filter"),
         buffer_(get_clock()), tf_(buffer_), up_and_running_(false) {}
 
   bool configure()

include/laser_filters/polygon_filter.h

@@ -55,7 +55,7 @@
 
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <tf2_ros/buffer.h>
+#include <tf2_ros/buffer.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rcl_interfaces/msg/set_parameters_result.hpp>
 
@@ -257,10 +257,10 @@ class LaserScanPolygonFilterBase : public filters::FilterBase<sensor_msgs::msg::
     }
     polygon_ = makePolygonFromString(polygon_string, polygon_);
     padPolygon(polygon_, polygon_padding_);
-    
+
     polygon_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>("polygon", rclcpp::QoS(1).transient_local().keep_last(1));
     is_polygon_published_ = false;
-    
+
     return true;
   }
 
@@ -288,7 +288,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBase<sensor_msgs::msg::
   bool invert_filter_;
   std::string footprint_topic_;
   bool is_polygon_published_ = false;
-  
+
 
   // tf listener to transform scans into the right frame
   tf2_ros::Buffer buffer_;
@@ -385,7 +385,7 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase {
       polygon_frame_,
       input_scan.header.frame_id,
       rclcpp::Time(input_scan.header.stamp) + std::chrono::duration<double>(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<std::recursive_mutex> 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<rclcpp::Parameter> 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,

src/generic_laser_filter_node.cpp

@@ -31,8 +31,8 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 
 // TF
-#include <tf2_ros/transform_listener.h>
-#include "tf2_ros/message_filter.h"
+#include <tf2_ros/transform_listener.hpp>
+#include <tf2_ros/message_filter.hpp>
 
 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;

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")
 {

src/scan_to_cloud_filter_chain.hpp

@@ -41,11 +41,11 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 
 // TF
-#include <tf2_ros/transform_listener.h>
-#include "tf2_ros/message_filter.h"
-#include "tf2_ros/create_timer_ros.h"
+#include <tf2_ros/transform_listener.hpp>
+#include <tf2_ros/message_filter.hpp>
+#include <tf2_ros/create_timer_ros.hpp>
 
-#include "message_filters/subscriber.hpp"
+#include <message_filters/subscriber.hpp>
 
 // Laser projection
 #include <laser_geometry/laser_geometry.hpp>

src/scan_to_scan_filter_chain.cpp

@@ -75,7 +75,7 @@ ScanToScanFilterChain::ScanToScanFilterChain(
     tf_filter_.reset(
       new tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>(
         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<double>(tf_filter_tolerance_));
 

src/scan_to_scan_filter_chain.hpp

@@ -34,10 +34,10 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 
 // TF
-#include <tf2_ros/transform_listener.h>
-#include "tf2_ros/message_filter.h"
+#include <tf2_ros/transform_listener.hpp>
+#include <tf2_ros/message_filter.hpp>
 
-#include "message_filters/subscriber.hpp"
+#include <message_filters/subscriber.hpp>
 
 #include "filters/filter_chain.hpp"