Replace TF message filter with explicit canTransform checks to avoid …

…cases where all scan messages get filtered due to delayed tf transforms.

include/slam_toolbox/slam_toolbox_common.hpp

@@ -30,11 +30,9 @@
 #include <fstream>
 
 #include "rclcpp/rclcpp.hpp"
-#include "message_filters/subscriber.h"
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/message_filter.h"
 #include "tf2/LinearMath/Matrix3x3.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
@@ -100,6 +98,7 @@ class SlamToolbox : public rclcpp::Node
     karto::Pose2 & karto_pose);
   karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder * laser, PosedScan & scanWPose);
   bool updateMap();
+  bool waitForTransform(const std::string& scan_frame, const rclcpp::Time& stamp);
   tf2::Stamped<tf2::Transform> setTransformFromPoses(
     const karto::Pose2 & pose,
     const karto::Pose2 & karto_pose, const rclcpp::Time & t,
@@ -130,8 +129,7 @@ class SlamToolbox : public rclcpp::Node
   std::unique_ptr<tf2_ros::Buffer> tf_;
   std::unique_ptr<tf2_ros::TransformListener> tfL_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
-  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> scan_filter_sub_;
-  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> scan_filter_;
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> sst_;
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> sstm_;
   std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>> pose_pub_;

src/experimental/slam_toolbox_lifelong.cpp

@@ -83,6 +83,10 @@ void LifelongSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) {
+    return;
+  }
+
   // store scan header
   scan_header = scan->header;
   // no odom info

src/slam_toolbox_async.cpp

@@ -35,6 +35,10 @@ void AsynchronousSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) {
+    return;
+  }
+
   // store scan header
   scan_header = scan->header;
   // no odom info

src/slam_toolbox_common.cpp

@@ -212,15 +212,9 @@ void SlamToolbox::setROSInterfaces()
     std::bind(&SlamToolbox::deserializePoseGraphCallback, this,
     std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
-  scan_filter_sub_ =
-    std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
-    shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data);
-  scan_filter_ =
-    std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
-    *scan_filter_sub_, *tf_, odom_frame_, 1, shared_from_this(),
-    tf2::durationFromSec(transform_timeout_.seconds()));
-  scan_filter_->registerCallback(
-    std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1));
+  scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+    scan_topic_,
+    rclcpp::SensorDataQoS().keep_last(1), std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1));
 }
 
 /*****************************************************************************/
@@ -396,6 +390,29 @@ bool SlamToolbox::updateMap()
   return true;
 }
 
+/*****************************************************************************/
+bool SlamToolbox::waitForTransform(const std::string& scan_frame, const rclcpp::Time& stamp)
+/*****************************************************************************/
+{
+  if (!tf_->_frameExists(odom_frame_)) {
+    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "tf frame [%s] doesn't exist yet.'", odom_frame_.c_str());
+    return false;
+  }
+
+  if (!tf_->_frameExists(scan_frame)) {
+    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "tf frame [%s] doesn't exist yet.'", scan_frame.c_str());
+    return false;
+  }
+
+  if (!tf_->canTransform(odom_frame_, scan_frame, stamp, transform_timeout_)) {
+    RCLCPP_WARN(get_logger(), "Failed to get transform %s -> %s.", scan_frame.c_str(), odom_frame_.c_str());
+    return false;
+  }
+
+  return true;
+}
+
+
 /*****************************************************************************/
 tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
   const Pose2 & corrected_pose,

src/slam_toolbox_localization.cpp

@@ -118,6 +118,10 @@ void LocalizationSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) {
+    return;
+  }
+
   // store scan header
   scan_header = scan->header;
   // no odom info

src/slam_toolbox_sync.cpp

@@ -65,6 +65,10 @@ void SynchronousSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  if (!waitForTransform(scan->header.frame_id, scan->header.stamp)) {
+    return;
+  }
+
   // store scan header
   scan_header = scan->header;
   // no odom info