Support both geometry_msgs::TwistStamped and geometry_msgs::Twist as inputs and output

include/twist_mux/topic_handle.hpp

@@ -154,69 +154,38 @@ class TopicHandle_
   T msg_;
 };
 
-class VelocityTopicHandle : public TopicHandle_<geometry_msgs::msg::Twist>
+template<typename T>
+class VelocityTopicHandle : public TopicHandle_<T>
 {
 private:
-  typedef TopicHandle_<geometry_msgs::msg::Twist> base_type;
+  typedef TopicHandle_<T> base_type;
 
 public:
+  using base_type::subscriber_;
+  using base_type::mux_;
+  using base_type::topic_;
+  using base_type::stamp_;
+  using base_type::msg_;
+
   typedef typename base_type::priority_type priority_type;
 
   VelocityTopicHandle(
     const std::string & name, const std::string & topic, const rclcpp::Duration & timeout,
     priority_type priority, TwistMux * mux)
   : base_type(name, topic, timeout, priority, mux)
   {
-    subscriber_ = mux_->create_subscription<geometry_msgs::msg::Twist>(
+    subscriber_ = mux_->template create_subscription<T>(
       topic_, rclcpp::SystemDefaultsQoS(),
       std::bind(&VelocityTopicHandle::callback, this, std::placeholders::_1));
   }
 
   bool isMasked(priority_type lock_priority) const
   {
     // std::cout << hasExpired() << " / " << (getPriority() < lock_priority) << std::endl;
-    return hasExpired() || (getPriority() < lock_priority);
-  }
-
-  void callback(const geometry_msgs::msg::Twist::ConstSharedPtr msg)
-  {
-    stamp_ = mux_->now();
-    msg_ = *msg;
-
-    // Check if this twist has priority.
-    // Note that we have to check all the locks because they might time out
-    // and since we have several topics we must look for the highest one in
-    // all the topic list; so far there's no O(1) solution.
-    if (mux_->hasPriority(*this)) {
-      mux_->publishTwist(msg);
-    }
-  }
-};
-
-class VelocityStampedTopicHandle : public TopicHandle_<geometry_msgs::msg::TwistStamped>
-{
-private:
-  typedef TopicHandle_<geometry_msgs::msg::TwistStamped> base_type;
-
-public:
-  typedef typename base_type::priority_type priority_type;
-
-  VelocityStampedTopicHandle(
-    const std::string & name, const std::string & topic, const rclcpp::Duration & timeout,
-    priority_type priority, TwistMux * mux)
-  : base_type(name, topic, timeout, priority, mux)
-  {
-    subscriber_ = mux_->create_subscription<geometry_msgs::msg::TwistStamped>(
-      topic_, rclcpp::SystemDefaultsQoS(),
-      std::bind(&VelocityStampedTopicHandle::callback, this, std::placeholders::_1));
-  }
-
-  bool isMasked(priority_type lock_priority) const
-  {
-    return hasExpired() || (getPriority() < lock_priority);
+    return base_type::hasExpired() || (base_type::getPriority() < lock_priority);
   }
 
-  void callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
+  void callback(const typename T::ConstSharedPtr msg)
   {
     stamp_ = mux_->now();
     msg_ = *msg;
@@ -225,8 +194,8 @@ class VelocityStampedTopicHandle : public TopicHandle_<geometry_msgs::msg::Twist
     // Note that we have to check all the locks because they might time out
     // and since we have several topics we must look for the highest one in
     // all the topic list; so far there's no O(1) solution.
-    if (mux_->hasPriorityStamped(*this)) {
-      mux_->publishTwistStamped(msg);
+    if (mux_->template hasPriority(*this)) {
+      mux_->template publishTwist(msg);
     }
   }
 };
@@ -244,7 +213,7 @@ class LockTopicHandle : public TopicHandle_<std_msgs::msg::Bool>
     priority_type priority, TwistMux * mux)
   : base_type(name, topic, timeout, priority, mux)
   {
-    subscriber_ = mux_->create_subscription<std_msgs::msg::Bool>(
+    subscriber_ = mux_->template create_subscription<std_msgs::msg::Bool>(
       topic_, rclcpp::SystemDefaultsQoS(),
       std::bind(&LockTopicHandle::callback, this, std::placeholders::_1));
   }

include/twist_mux/twist_mux.hpp

@@ -51,8 +51,8 @@ namespace twist_mux
 // Forwarding declarations:
 class TwistMuxDiagnostics;
 struct TwistMuxDiagnosticsStatus;
+template <typename T>
 class VelocityTopicHandle;
-class VelocityStampedTopicHandle;
 class LockTopicHandle;
 
 /**
@@ -64,23 +64,24 @@ class TwistMux : public rclcpp::Node
 public:
   template<typename T>
   using handle_container = std::list<T>;
+  using velocity_handle_variant = std::variant<VelocityTopicHandle<geometry_msgs::msg::Twist>, VelocityTopicHandle<geometry_msgs::msg::TwistStamped>>;
+  using publisher_variant = std::variant<rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr, rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr>;
+  using message_variant = std::variant<geometry_msgs::msg::Twist, geometry_msgs::msg::TwistStamped>;
 
-  using velocity_topic_container = handle_container<VelocityTopicHandle>;
-  using velocity_stamped_topic_container = handle_container<VelocityStampedTopicHandle>;
+  using velocity_topic_container = handle_container<velocity_handle_variant>;
   using lock_topic_container = handle_container<LockTopicHandle>;
 
   TwistMux();
   ~TwistMux() = default;
 
   void init();
 
-  bool hasPriority(const VelocityTopicHandle & twist);
+  template <typename VelocityTopicHandleT>
+  bool hasPriority(const VelocityTopicHandleT & twist);
 
-  bool hasPriorityStamped(const VelocityStampedTopicHandle & twist);
-
-  void publishTwist(const geometry_msgs::msg::Twist::ConstSharedPtr & msg);
-
-  void publishTwistStamped(const geometry_msgs::msg::TwistStamped::ConstSharedPtr & msg);
+
+  template <typename MessageConstSharedPtrT>
+  void publishTwist(const MessageConstSharedPtrT & msg);
 
   void updateDiagnostics();
 
@@ -99,14 +100,12 @@ class TwistMux : public rclcpp::Node
    * must reserve the number of handles initially.
    */
   std::shared_ptr<velocity_topic_container> velocity_hs_;
-  std::shared_ptr<velocity_stamped_topic_container> velocity_stamped_hs_;
   std::shared_ptr<lock_topic_container> lock_hs_;
 
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_pub_stamped_;
+  publisher_variant cmd_pub_;
+  message_variant last_cmd_;
 
-  geometry_msgs::msg::Twist last_cmd_;
-  geometry_msgs::msg::TwistStamped last_cmd_stamped_;
+  bool output_stamped;
 
   template<typename T>
   void getTopicHandles(const std::string & param_name, handle_container<T> & topic_hs);

include/twist_mux/twist_mux_diagnostics_status.hpp

@@ -55,21 +55,16 @@ struct TwistMuxDiagnosticsStatus
 
   LockTopicHandle::priority_type priority;
 
-  bool use_stamped;
-
   std::shared_ptr<TwistMux::velocity_topic_container> velocity_hs;
-  std::shared_ptr<TwistMux::velocity_stamped_topic_container> velocity_stamped_hs;
   std::shared_ptr<TwistMux::lock_topic_container> lock_hs;
 
   TwistMuxDiagnosticsStatus()
   : reading_age(0),
     last_loop_update(rclcpp::Clock().now()),
     main_loop_time(0),
-    priority(0),
-    use_stamped(true)
+    priority(0)
   {
     velocity_hs = std::make_shared<TwistMux::velocity_topic_container>();
-    velocity_stamped_hs = std::make_shared<TwistMux::velocity_stamped_topic_container>();
     lock_hs = std::make_shared<TwistMux::lock_topic_container>();
   }
 };

launch/twist_mux_launch.py

@@ -52,13 +52,18 @@ def generate_launch_description():
             'use_sim_time',
             default_value='False',
             description='Use simulation time'),
+        DeclareLaunchArgument(
+            'output_stamped',
+            default_value=False,
+            description='Output as geometry_msgs/TwistStamped instead of geometry_msgs/Twist'),
         Node(
             package='twist_mux',
             executable='twist_mux',
             output='screen',
             remappings={('/cmd_vel_out', LaunchConfiguration('cmd_vel_out'))},
             parameters=[
-                {'use_sim_time': LaunchConfiguration('use_sim_time')},
+                {'use_sim_time': LaunchConfiguration('use_sim_time'),
+                 'output_stamped': LaunchConfiguration('output_stamped')},
                 LaunchConfiguration('config_locks'),
                 LaunchConfiguration('config_topics')]
         ),

src/twist_marker.cpp

@@ -175,4 +175,4 @@ int main(int argc, char * argv[])
   rclcpp::shutdown();
 
   return EXIT_SUCCESS;
-}
+}