From 591f6f2871db32da2bf07a020972f519682990cb Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Tue, 2 May 2023 15:02:19 +0200 Subject: [PATCH 1/6] Update twist_mux.cpp remove lock test in `hasPriority` --- src/twist_mux.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/twist_mux.cpp b/src/twist_mux.cpp index 8dcbc40..8cc4b81 100644 --- a/src/twist_mux.cpp +++ b/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 @@ -167,15 +167,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; From 6041671e33ddd2bf64dd4a6cb23649607daee1a9 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Tue, 2 May 2023 15:03:35 +0200 Subject: [PATCH 2/6] Update twist_mux.hpp Move getLockPriority to public --- include/twist_mux/twist_mux.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/twist_mux/twist_mux.hpp b/include/twist_mux/twist_mux.hpp index dbb47e8..d88544c 100644 --- a/include/twist_mux/twist_mux.hpp +++ b/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 void getTopicHandles(const std::string & param_name, handle_container & topic_hs); - int getLockPriority(); - std::shared_ptr diagnostics_; std::shared_ptr status_; }; From e8b0e85726a9705addd168822946282d66181c6d Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Tue, 2 May 2023 15:05:00 +0200 Subject: [PATCH 3/6] Update topic_handle.hpp Publish zero if topic has priority and is locked --- include/twist_mux/topic_handle.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/include/twist_mux/topic_handle.hpp b/include/twist_mux/topic_handle.hpp index 5dc2015..e632a0e 100644 --- a/include/twist_mux/topic_handle.hpp +++ b/include/twist_mux/topic_handle.hpp @@ -189,12 +189,18 @@ class VelocityTopicHandle : public TopicHandle_ 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(); + } + // 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_); } } }; From 2dbc3f0189b19a86db7106377b4edd1b8faf51d8 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Mon, 2 Oct 2023 10:44:32 +0200 Subject: [PATCH 4/6] Update twist_mux.cpp get `pub_zer_if_locked` param and propagate --- src/twist_mux.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/twist_mux.cpp b/src/twist_mux.cpp index 8cc4b81..4505ffd 100644 --- a/src/twist_mux.cpp +++ b/src/twist_mux.cpp @@ -126,18 +126,26 @@ void TwistMux::getTopicHandles(const std::string & param_name, std::list & to std::string topic; double timeout = 0; int priority = 0; + bool pub_zero_if_locked = false; auto nh = std::shared_ptr(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(timeout), priority, this); + topic_hs.emplace_back( + prefix, topic, std::chrono::duration( + 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()); From 149375495eee61750f63764f486049033ee47fb2 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Mon, 2 Oct 2023 10:46:14 +0200 Subject: [PATCH 5/6] Update topic_handle.hpp Add `pub_zero_if_locked` parameter ti TopicHandle class. Publish zero vel only if param is true. --- include/twist_mux/topic_handle.hpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/include/twist_mux/topic_handle.hpp b/include/twist_mux/topic_handle.hpp index e632a0e..847c9be 100644 --- a/include/twist_mux/topic_handle.hpp +++ b/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_ @@ -166,8 +169,8 @@ class VelocityTopicHandle : public TopicHandle_ 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( topic_, rclcpp::SystemDefaultsQoS(), @@ -193,6 +196,7 @@ class VelocityTopicHandle : public TopicHandle_ 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. @@ -218,8 +222,8 @@ class LockTopicHandle : public TopicHandle_ 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( topic_, rclcpp::SystemDefaultsQoS(), From a3044dafd67686139ce6787389cbe934b28caf13 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Mon, 2 Oct 2023 10:51:24 +0200 Subject: [PATCH 6/6] Update twist_mux_topics.yaml add parameter to config file --- config/twist_mux_topics.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/config/twist_mux_topics.yaml b/config/twist_mux_topics.yaml index 45628d4..607af7b 100644 --- a/config/twist_mux_topics.yaml +++ b/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