Transform sensor updates

fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp

@@ -79,7 +79,7 @@ struct TransformSensorParams : public ParameterBase
     throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"),
                                                  throttle_use_wall_time);
 
-    fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic);
+    transforms = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "transforms"), transforms);
 
     target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame);
 
@@ -96,7 +96,7 @@ struct TransformSensorParams : public ParameterBase
   bool throttle_use_wall_time{ false };      //!< Whether to throttle using ros::WallTime or not
   std::vector<double> pose_covariance;       //!< The diagonal elements of the tag pose covariance
   int queue_size{ 10 };
-  std::string topic;
+  std::vector<std::string> transforms;
   std::string target_frame;
   std::vector<size_t> position_indices;
   std::vector<size_t> orientation_indices;

fuse_models/include/fuse_models/transform_sensor.hpp

@@ -50,7 +50,6 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
-#include "tf2_msgs/msg/tf_message.hpp"
 
 namespace fuse_models
 {
@@ -143,6 +142,7 @@ class TransformSensor : public fuse_core::AsyncSensorModel
 
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::set<std::string> transforms_of_interest_;
 
   rclcpp::Subscription<MessageType>::SharedPtr sub_;
 

fuse_models/src/transform_sensor.cpp

@@ -32,6 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 #include <tf2_ros/buffer.h>
+#include <tf2_ros/message_filter.h>
 #include <memory>
 
 #include <fuse_core/transaction.hpp>
@@ -85,8 +86,19 @@ void TransformSensor::onInit()
 
   if (params_.position_indices.empty() && params_.orientation_indices.empty())
   {
-    throw std::runtime_error("No dimensions specified, so this sensor would not do anything (data from topic " +
-                             params_.topic + " would be ignored).");
+    throw std::runtime_error(
+        "No dimensions specified, so this sensor would not do anything (tf data would be ignored).");
+  }
+
+  if (params_.transforms.empty())
+  {
+    throw std::runtime_error(
+        "No transforms specified, this sensor would not do anything (all tf data would be ignored).");
+  }
+
+  for (auto const& name : params_.transforms)
+  {
+    transforms_of_interest_.insert(name);
   }
 
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(clock_);
@@ -98,7 +110,7 @@ void TransformSensor::onStart()
   rclcpp::SubscriptionOptions sub_options;
   sub_options.callback_group = cb_group_;
 
-  sub_ = rclcpp::create_subscription<MessageType>(interfaces_, params_.topic, params_.queue_size,
+  sub_ = rclcpp::create_subscription<MessageType>(interfaces_, "/tf", params_.queue_size,
                                                   std::bind(&AprilTagThrottledCallback::callback<const MessageType&>,
                                                             &throttled_callback_, std::placeholders::_1),
                                                   sub_options);
@@ -113,6 +125,14 @@ void TransformSensor::process(MessageType const& msg)
 {
   for (auto const& transform : msg.transforms)
   {
+    std::string const& tf_name = transform.child_frame_id;
+    if (transforms_of_interest_.find(tf_name) == transforms_of_interest_.end())
+    {
+      // we don't care about this transform, skip it
+      RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(), tf_name.c_str());
+      continue;
+    }
+    RCLCPP_DEBUG(logger_, "Got transform of interest from %s to %s", transform.header.frame_id.c_str(), tf_name.c_str());
     // Create a transaction object
     auto transaction = fuse_core::Transaction::make_shared();
     transaction->stamp(transform.header.stamp);