Create a transform subscribers to enable virtual joints (#310)

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h

@@ -45,7 +45,6 @@
 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
 #include <moveit/robot_state/robot_state.h>
 #include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
 
 namespace moveit_cpp
 {
@@ -170,7 +169,6 @@ class MoveItCpp
 
   // TF
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
   // Planning
   std::map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

@@ -55,7 +55,6 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options
 {
   if (!tf_buffer_)
     tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
   // Configure planning scene monitor
   if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options))
@@ -303,7 +302,6 @@ const std::shared_ptr<tf2_ros::Buffer>& MoveItCpp::getTFBuffer() const
 
 void MoveItCpp::clearContents()
 {
-  tf_listener_.reset();
   tf_buffer_.reset();
   planning_scene_monitor_.reset();
   robot_model_.reset();

moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp

@@ -36,7 +36,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
-#include <tf2_ros/transform_listener.h>
 #include <memory>
 
 static const std::string ROBOT_DESCRIPTION = "robot_description";
@@ -52,8 +51,6 @@ int main(int argc, char** argv)
   double lifetime = 600.0;
 
   std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener =
-      std::make_shared<tf2_ros::TransformListener>(*tf_buffer, node);
   planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION, tf_buffer);
   if (psm.getPlanningScene())
   {

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h

@@ -46,6 +46,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
 #include <tf2_ros/buffer.h>
 
 #include <moveit/robot_state/robot_state.h>
@@ -58,15 +60,15 @@ using JointStateUpdateCallback = std::function<void(sensor_msgs::msg::JointState
     @brief Monitors the joint_states topic and tf to maintain the current state of the robot. */
 class CurrentStateMonitor
 {
-  using TFConnection = boost::signals2::connection;
-
 public:
   /**
    * @brief      Dependency injection class for testing the CurrentStateMonitor
    */
   class MiddlewareHandle
   {
   public:
+    using TfCallback = std::function<void(const tf2_msgs::msg::TFMessage::ConstSharedPtr)>;
+
     /**
      * @brief      Destroys the object.
      */
@@ -87,6 +89,20 @@ class CurrentStateMonitor
      */
     virtual void createJointStateSubscription(const std::string& topic, JointStateUpdateCallback callback) = 0;
 
+    /**
+     * @brief      Creates a static transform message subscription
+     *
+     * @param[in]  callback  The callback
+     */
+    virtual void createStaticTfSubscription(TfCallback callback) = 0;
+
+    /**
+     * @brief      Creates a dynamic transform message subscription
+     *
+     * @param[in]  callback  The callback
+     */
+    virtual void createDynamicTfSubscription(TfCallback callback) = 0;
+
     /**
      * @brief      Reset the joint state subscription
      */
@@ -107,6 +123,25 @@ class CurrentStateMonitor
      * @return     True if sleep happened as expected.
      */
     virtual bool sleepFor(const std::chrono::nanoseconds& nanoseconds) const = 0;
+
+    /**
+     * @brief      Get the static transform topic name
+     *
+     * @return     The static transform topic name.
+     */
+    virtual std::string getStaticTfTopicName() const = 0;
+
+    /**
+     * @brief      Get the dynamic transform topic name
+     *
+     * @return     The dynamic transform topic name.
+     */
+    virtual std::string getDynamicTfTopicName() const = 0;
+
+    /**
+     * @brief      Reset the static & dynamic transform subscriptions
+     */
+    virtual void resetTfSubscriptions() = 0;
   };
 
   /** @brief Constructor.
@@ -279,7 +314,8 @@ class CurrentStateMonitor
                                std::vector<std::string>* missing_joints) const;
 
   void jointStateCallback(sensor_msgs::msg::JointState::ConstSharedPtr joint_state);
-  void tfCallback();
+  void updateMultiDofJoints();
+  void transformCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg, const bool is_static);
 
   std::unique_ptr<MiddlewareHandle> middleware_handle_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -295,8 +331,6 @@ class CurrentStateMonitor
   mutable std::mutex state_update_lock_;
   mutable std::condition_variable state_update_condition_;
   std::vector<JointStateUpdateCallback> update_callbacks_;
-
-  std::shared_ptr<TFConnection> tf_connection_;
 };
 
 MOVEIT_CLASS_FORWARD(CurrentStateMonitor);  // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp

@@ -74,6 +74,21 @@ class CurrentStateMonitorMiddlewareHandle : public CurrentStateMonitor::Middlewa
    */
   void createJointStateSubscription(const std::string& topic, JointStateUpdateCallback callback) override;
 
+  /**
+   * @brief      Creates a static transform message subscription
+   *
+   * @param[in]  callback  The callback
+   */
+  void createStaticTfSubscription(std::function<void(const tf2_msgs::msg::TFMessage::ConstSharedPtr)> callback) override;
+
+  /**
+   * @brief      Creates a dynamic transform message subscription
+   *
+   * @param[in]  callback  The callback
+   */
+  void
+  createDynamicTfSubscription(std::function<void(const tf2_msgs::msg::TFMessage::ConstSharedPtr)> callback) override;
+
   /**
    * @brief      Reset the joint state subscription
    */
@@ -95,9 +110,30 @@ class CurrentStateMonitorMiddlewareHandle : public CurrentStateMonitor::Middlewa
    */
   bool sleepFor(const std::chrono::nanoseconds& nanoseconds) const override;
 
+  /**
+   * @brief      Get the static transform topic name
+   *
+   * @return     The static transform topic name.
+   */
+  std::string getStaticTfTopicName() const override;
+
+  /**
+   * @brief      Get the dynamic transform topic name
+   *
+   * @return     The dynamic transform topic name.
+   */
+  std::string getDynamicTfTopicName() const override;
+
+  /**
+   * @brief      Reset the static & dynamic transform subscriptions
+   */
+  void resetTfSubscriptions() override;
+
 private:
   rclcpp::Node::SharedPtr node_;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscription_;
+  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr transform_subscriber_;
+  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr static_transform_subscriber_;
 };
 
 }  // namespace planning_scene_monitor

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

@@ -159,10 +159,20 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi
     }
     if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
     {
-      // TODO (anasarrak): replace this for the appropiate function, there is no similar
-      // function in ros2/geometry2.
-      // tf_connection_.reset(new TFConnection(
-      //     tf_buffer_->_addTransformsChangedListener(std::bind(&CurrentStateMonitor::tfCallback, this))));
+      // If a dedicated thread is enabled for the buffer this probably means the user is adding them either through
+      // tf2_ros::TransformListener or themselves, so we print a warning message warning that transformCallback is doing
+      // the same duplicate operation
+      if (tf_buffer_->isUsingDedicatedThread())
+      {
+        RCLCPP_WARN(LOGGER, "The tf2_ros::Buffer is attached to tf2_ros::TransformListener and the internal tf "
+                            "subscribers inside CurrentStateMonitor, you may want to remove the transform listener to "
+                            "avoid duplicate addition to the same transforms");
+      }
+      tf_buffer_->setUsingDedicatedThread(true);
+      middleware_handle_->createDynamicTfSubscription(
+          std::bind(&CurrentStateMonitor::transformCallback, this, std::placeholders::_1, false));
+      middleware_handle_->createStaticTfSubscription(
+          std::bind(&CurrentStateMonitor::transformCallback, this, std::placeholders::_1, true));
     }
     state_monitor_started_ = true;
     monitor_start_time_ = middleware_handle_->now();
@@ -180,12 +190,9 @@ void CurrentStateMonitor::stopStateMonitor()
   if (state_monitor_started_)
   {
     middleware_handle_->resetJointStateSubscription();
-    if (tf_buffer_ && tf_connection_)
+    if (tf_buffer_)
     {
-      // TODO (anasarrak): replace this for the appropiate function, there is no similar
-      // function in ros2/geometry2.
-      // tf_buffer_->_removeTransformsChangedListener(*tf_connection_);
-      tf_connection_.reset();
+      middleware_handle_->resetTfSubscriptions();
     }
     RCLCPP_DEBUG(LOGGER, "No longer listening for joint states");
     state_monitor_started_ = false;
@@ -315,7 +322,7 @@ void CurrentStateMonitor::jointStateCallback(sensor_msgs::msg::JointState::Const
       if (jm->getVariableCount() != 1)
         continue;
 
-      joint_time_[jm] = joint_state->header.stamp;
+      joint_time_.insert_or_assign(jm, joint_state->header.stamp);
 
       if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
       {
@@ -369,7 +376,7 @@ void CurrentStateMonitor::jointStateCallback(sensor_msgs::msg::JointState::Const
   state_update_condition_.notify_all();
 }
 
-void CurrentStateMonitor::tfCallback()
+void CurrentStateMonitor::updateMultiDofJoints()
 {
   // read multi-dof joint states from TF, if needed
   const std::vector<const moveit::core::JointModel*>& multi_dof_joints = robot_model_->getMultiDOFJointModels();
@@ -384,7 +391,7 @@ void CurrentStateMonitor::tfCallback()
       const std::string& parent_frame =
           joint->getParentLinkModel() ? joint->getParentLinkModel()->getName() : robot_model_->getModelFrame();
 
-      rclcpp::Time latest_common_time;
+      rclcpp::Time latest_common_time(0, 0, RCL_ROS_TIME);
       geometry_msgs::msg::TransformStamped transf;
       try
       {
@@ -401,8 +408,11 @@ void CurrentStateMonitor::tfCallback()
         continue;
       }
 
+      if (auto joint_time_it = joint_time_.find(joint); joint_time_it == joint_time_.end())
+        joint_time_.emplace(joint, rclcpp::Time(0, 0, RCL_ROS_TIME));
+
       // allow update if time is more recent or if it is a static transform (time = 0)
-      if (latest_common_time <= joint_time_[joint] && latest_common_time > rclcpp::Time(0))
+      if (latest_common_time <= joint_time_[joint] && latest_common_time > rclcpp::Time(0, 0, RCL_ROS_TIME))
         continue;
       joint_time_[joint] = latest_common_time;
 
@@ -441,4 +451,25 @@ void CurrentStateMonitor::tfCallback()
   }
 }
 
+// Copied from https://github.com/ros2/geometry2/blob/ros2/tf2_ros/src/transform_listener.cpp
+void CurrentStateMonitor::transformCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg, const bool is_static)
+{
+  for (const auto& transform : msg->transforms)
+  {
+    try
+    {
+      tf_buffer_->setTransform(transform,
+                               is_static ? middleware_handle_->getStaticTfTopicName() :
+                                           middleware_handle_->getDynamicTfTopicName(),
+                               is_static);
+    }
+    catch (tf2::TransformException& ex)
+    {
+      std::string temp = ex.what();
+      RCLCPP_ERROR(LOGGER, "Failure to set recieved transform from %s to %s with error: %s\n",
+                   transform.child_frame_id.c_str(), transform.header.frame_id.c_str(), temp.c_str());
+    }
+  }
+  updateMultiDofJoints();
+}
 }  // namespace planning_scene_monitor

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp

@@ -36,6 +36,7 @@
 
 #include <chrono>
 #include <string>
+#include <tf2_ros/qos.hpp>
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -87,4 +88,32 @@ bool CurrentStateMonitorMiddlewareHandle::sleepFor(const std::chrono::nanosecond
   return rclcpp::sleep_for(nanoseconds);
 }
 
+void CurrentStateMonitorMiddlewareHandle::createStaticTfSubscription(TfCallback callback)
+{
+  static_transform_subscriber_ =
+      node_->create_subscription<tf2_msgs::msg::TFMessage>("/tf", tf2_ros::DynamicListenerQoS(), callback);
+}
+
+void CurrentStateMonitorMiddlewareHandle::createDynamicTfSubscription(TfCallback callback)
+{
+  transform_subscriber_ =
+      node_->create_subscription<tf2_msgs::msg::TFMessage>("/tf_static", tf2_ros::StaticListenerQoS(), callback);
+}
+
+std::string CurrentStateMonitorMiddlewareHandle::getStaticTfTopicName() const
+{
+  return static_transform_subscriber_ ? static_transform_subscriber_->get_topic_name() : "";
+}
+
+std::string CurrentStateMonitorMiddlewareHandle::getDynamicTfTopicName() const
+{
+  return transform_subscriber_ ? transform_subscriber_->get_topic_name() : "";
+}
+
+void CurrentStateMonitorMiddlewareHandle::resetTfSubscriptions()
+{
+  transform_subscriber_.reset();
+  static_transform_subscriber_.reset();
+}
+
 }  // namespace planning_scene_monitor

moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp

@@ -55,6 +55,11 @@ struct MockMiddlewareHandle : public planning_scene_monitor::CurrentStateMonitor
   MOCK_METHOD(void, resetJointStateSubscription, (), (override));
   MOCK_METHOD(std::string, getJointStateTopicName, (), (const, override));
   MOCK_METHOD(bool, sleepFor, (const std::chrono::nanoseconds& nanoseconds), (const, override));
+  MOCK_METHOD(void, createStaticTfSubscription, (TfCallback callback), (override));
+  MOCK_METHOD(void, createDynamicTfSubscription, (TfCallback callback), (override));
+  MOCK_METHOD(std::string, getStaticTfTopicName, (), (const, override));
+  MOCK_METHOD(std::string, getDynamicTfTopicName, (), (const, override));
+  MOCK_METHOD(void, resetTfSubscriptions, (), (override));
 };
 
 TEST(CurrentStateMonitorTests, StartCreateSubscriptionTest)

moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp

@@ -36,7 +36,6 @@
 
 #include <moveit/common_planning_interface_objects/common_objects.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
-#include <tf2_ros/transform_listener.h>
 
 using namespace planning_scene_monitor;
 using namespace robot_model_loader;
@@ -101,13 +100,10 @@ std::shared_ptr<tf2_ros::Buffer> getSharedTF()
   SharedStorage& s = getSharedStorage();
   boost::mutex::scoped_lock slock(s.lock_);
 
-  typedef CoupledDeleter<tf2_ros::Buffer, tf2_ros::TransformListener> Deleter;
   std::shared_ptr<tf2_ros::Buffer> buffer = s.tf_buffer_.lock();
   if (!buffer)
   {
-    tf2_ros::Buffer* raw = new tf2_ros::Buffer(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME));
-    // assign custom deleter to also delete associated TransformListener
-    buffer.reset(raw, Deleter(new tf2_ros::TransformListener(*raw)));
+    buffer = std::make_shared<tf2_ros::Buffer>(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME));
     s.tf_buffer_ = buffer;
   }
   return buffer;

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -149,7 +149,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
      it has to be of type ros::CallbackQueue
         (which is the default type of callback queues used in ROS)
       \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified,
-                        one will be constructed internally along with an internal TF2_ROS TransformListener
+                        one will be constructed internally
       \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting.
     */
   MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt,
@@ -160,7 +160,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
       \brief Construct a client for the MoveGroup action for a particular \e group.
 
       \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified,
-                        one will be constructed internally along with an internal TF2_ROS TransformListener
+                        one will be constructed internally
       \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting.
     */
   MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group,