rebase stamped-support

Author: 5730289021-NN

include/twist_mux/topic_handle.hpp

@@ -154,69 +154,41 @@ 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;
+
+  // https://index.ros.org/doc/ros2/About-Quality-of-Service-Settings
+  // rmw_qos_profile_t twist_qos_profile = rmw_qos_profile_sensor_data;
 
 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 +197,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 +216,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

@@ -34,7 +34,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <geometry_msgs/msg/twist.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -108,33 +107,21 @@ class TwistMarkerPublisher : public rclcpp::Node
   {
     std::string frame_id;
     double scale;
-    bool use_stamped;
     double z;
 
     this->declare_parameter("frame_id", "base_footprint");
     this->declare_parameter("scale", 1.0);
-    this->declare_parameter("use_stamped", false);
     this->declare_parameter("vertical_position", 2.0);
 
     this->get_parameter<std::string>("frame_id", frame_id);
     this->get_parameter<double>("scale", scale);
-    this->get_parameter<bool>("use_stamped", use_stamped);
     this->get_parameter<double>("vertical_position", z);
 
     marker_ = std::make_shared<TwistMarker>(frame_id, scale, z);
 
-    if (use_stamped)
-    {
-      sub_stamped_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
-        "twist", rclcpp::SystemDefaultsQoS(),
-        std::bind(&TwistMarkerPublisher::callback_stamped, this, std::placeholders::_1));
-    }
-    else
-    {
-      sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
-        "twist", rclcpp::SystemDefaultsQoS(),
-        std::bind(&TwistMarkerPublisher::callback, this, std::placeholders::_1));
-    }
+    sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+      "twist", rclcpp::SystemDefaultsQoS(),
+      std::bind(&TwistMarkerPublisher::callback, this, std::placeholders::_1));
 
     pub_ =
       this->create_publisher<visualization_msgs::msg::Marker>(
@@ -149,16 +136,8 @@ class TwistMarkerPublisher : public rclcpp::Node
     pub_->publish(marker_->getMarker());
   }
 
-  void callback_stamped(const geometry_msgs::msg::TwistStamped::ConstSharedPtr twist)
-  {
-    marker_->update(twist->twist);
-
-    pub_->publish(marker_->getMarker());
-  }
-
 private:
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_;
-  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_stamped_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_;
 
   std::shared_ptr<TwistMarker> marker_ = nullptr;