Publish zero if locked

config/twist_mux_topics.yaml

@@ -4,6 +4,7 @@
 # - topic   : input topic of geometry_msgs::Twist type
 # - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead
 # - priority: priority in the range [0, 255]; the higher the more priority over other topics
+# - pub_zero_if_locked: if true, publish 0.0 command if the topic is locked
 
 twist_mux:
   ros__parameters:
@@ -12,15 +13,19 @@ twist_mux:
         topic   : nav_vel
         timeout : 0.5
         priority: 10
+        pub_zero_if_locked: false
       joystick:
         topic   : joy_vel
         timeout : 0.5
         priority: 100
+        pub_zero_if_locked: false
       keyboard:
         topic   : key_vel
         timeout : 0.5
         priority: 90
+        pub_zero_if_locked: false
       tablet:
         topic   : tab_vel
         timeout : 0.5
         priority: 100
+        pub_zero_if_locked: false

include/twist_mux/topic_handle.hpp

@@ -75,11 +75,12 @@ class TopicHandle_
    */
   TopicHandle_(
     const std::string & name, const std::string & topic, const rclcpp::Duration & timeout,
-    priority_type priority, TwistMux * mux)
+    priority_type priority, bool pub_zero_if_locked, TwistMux * mux)
   : name_(name),
     topic_(topic),
     timeout_(timeout),
     priority_(clamp(priority, priority_type(0), priority_type(255))),
+    pub_zero_if_locked_(pub_zero_if_locked),
     mux_(mux),
     stamp_(0)
   {
@@ -151,6 +152,8 @@ class TopicHandle_
 
   rclcpp::Time stamp_;
   T msg_;
+
+  bool pub_zero_if_locked_;
 };
 
 class VelocityTopicHandle : public TopicHandle_<geometry_msgs::msg::Twist>
@@ -166,8 +169,8 @@ class VelocityTopicHandle : public TopicHandle_<geometry_msgs::msg::Twist>
 
   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)
+    priority_type priority, bool pub_zero_if_locked, TwistMux * mux)
+  : base_type(name, topic, timeout, priority, pub_zero_if_locked, mux)
   {
     subscriber_ = mux_->create_subscription<geometry_msgs::msg::Twist>(
       topic_, rclcpp::SystemDefaultsQoS(),
@@ -189,12 +192,19 @@ class VelocityTopicHandle : public TopicHandle_<geometry_msgs::msg::Twist>
     stamp_ = mux_->now();
     msg_ = *msg;
 
+    // If topic locked, overide with zero twist
+    const auto lock_priority = mux_->getLockPriority();
+    if (getPriority() < lock_priority) {
+      msg_ = geometry_msgs::msg::Twist();
+      if (!pub_zero_if_locked_) {return;}
+    }
+
     // 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);
+      mux_->publishTwist(msg_);
     }
   }
 };
@@ -212,8 +222,8 @@ class LockTopicHandle : public TopicHandle_<std_msgs::msg::Bool>
 
   LockTopicHandle(
     const std::string & name, const std::string & topic, const rclcpp::Duration & timeout,
-    priority_type priority, TwistMux * mux)
-  : base_type(name, topic, timeout, priority, mux)
+    priority_type priority, bool pub_zero_if_locked, TwistMux * mux)
+  : base_type(name, topic, timeout, priority, pub_zero_if_locked, mux)
   {
     subscriber_ = mux_->create_subscription<std_msgs::msg::Bool>(
       topic_, rclcpp::SystemDefaultsQoS(),

include/twist_mux/twist_mux.hpp

@@ -73,10 +73,12 @@ class TwistMux : public rclcpp::Node
 
   bool hasPriority(const VelocityTopicHandle & twist);
 
-  void publishTwist(const geometry_msgs::msg::Twist::ConstSharedPtr & msg);
+  void publishTwist(const geometry_msgs::msg::Twist & msg);
 
   void updateDiagnostics();
 
+  int getLockPriority();
+
 protected:
   typedef TwistMuxDiagnostics diagnostics_type;
   typedef TwistMuxDiagnosticsStatus status_type;
@@ -101,8 +103,6 @@ class TwistMux : public rclcpp::Node
   template<typename T>
   void getTopicHandles(const std::string & param_name, handle_container<T> & topic_hs);
 
-  int getLockPriority();
-
   std::shared_ptr<diagnostics_type> diagnostics_;
   std::shared_ptr<status_type> status_;
 };

src/twist_mux.cpp

@@ -107,9 +107,9 @@ void TwistMux::updateDiagnostics()
   diagnostics_->updateStatus(status_);
 }
 
-void TwistMux::publishTwist(const geometry_msgs::msg::Twist::ConstSharedPtr & msg)
+void TwistMux::publishTwist(const geometry_msgs::msg::Twist & msg)
 {
-  cmd_pub_->publish(*msg);
+  cmd_pub_->publish(msg);
 }
 
 template<typename T>
@@ -126,18 +126,26 @@ void TwistMux::getTopicHandles(const std::string & param_name, std::list<T> & to
       std::string topic;
       double timeout = 0;
       int priority = 0;
+      bool pub_zero_if_locked = false;
 
       auto nh = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
 
       fetch_param(nh, prefix + ".topic", topic);
       fetch_param(nh, prefix + ".timeout", timeout);
       fetch_param(nh, prefix + ".priority", priority);
+      try {
+        fetch_param(nh, prefix + ".pub_zero_if_locked", pub_zero_if_locked);
+      } catch (...) {
+      }
 
       RCLCPP_DEBUG(get_logger(), "Retrieved topic: %s", topic.c_str());
       RCLCPP_DEBUG(get_logger(), "Listed prefix: %.2f", timeout);
       RCLCPP_DEBUG(get_logger(), "Listed prefix: %d", priority);
+      RCLCPP_DEBUG(get_logger(), "Listed prefix: %d", pub_zero_if_locked);
 
-      topic_hs.emplace_back(prefix, topic, std::chrono::duration<double>(timeout), priority, this);
+      topic_hs.emplace_back(
+        prefix, topic, std::chrono::duration<double>(
+          timeout), priority, pub_zero_if_locked, this);
     }
   } catch (const ParamsHelperException & e) {
     RCLCPP_FATAL(get_logger(), "Error parsing params '%s':\n\t%s", param_name.c_str(), e.what());
@@ -167,15 +175,13 @@ int TwistMux::getLockPriority()
 
 bool TwistMux::hasPriority(const VelocityTopicHandle & twist)
 {
-  const auto lock_priority = getLockPriority();
-
   LockTopicHandle::priority_type priority = 0;
   std::string velocity_name = "NULL";
 
   /// max_element on the priority of velocity topic handles satisfying
   /// that is NOT masked by the lock priority:
   for (const auto & velocity_h : *velocity_hs_) {
-    if (!velocity_h.isMasked(lock_priority)) {
+    if (!velocity_h.hasExpired()) {
       const auto velocity_priority = velocity_h.getPriority();
       if (priority < velocity_priority) {
         priority = velocity_priority;