add parameter for setting publish period

Author: mamueluth

controller_manager/include/controller_manager/controller_manager.hpp

@@ -190,13 +190,20 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   unsigned int get_update_rate() const;
 
+  // Per controller update rate support
+  CONTROLLER_MANAGER_PUBLIC
+  std::chrono::milliseconds distributed_interfaces_publish_period() const;
+
 protected:
   CONTROLLER_MANAGER_PUBLIC
   void init_services();
 
   CONTROLLER_MANAGER_PUBLIC
   void configure_controller_manager();
 
+  CONTROLLER_MANAGER_PUBLIC
+  void init_distributed_sub_controller_manager();
+
   CONTROLLER_MANAGER_PUBLIC
   void init_distributed_main_controller_services();
 
@@ -422,6 +429,7 @@ class ControllerManager : public rclcpp::Node
 
   bool distributed_ = false;
   bool sub_controller_manager_ = false;
+  std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12);
 
   rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_;
   /**

controller_manager/src/controller_manager.cpp

@@ -329,9 +329,7 @@ void ControllerManager::configure_controller_manager()
   bool central_controller_manager = distributed_ && !sub_controller_manager_;
   if (distributed_sub_controller_manager)
   {
-    add_hardware_state_publishers();
-    add_hardware_command_forwarders();
-    register_sub_controller_manager();
+    init_distributed_sub_controller_manager();
   }
   // This means we are the central controller manager
   else if (central_controller_manager)
@@ -350,6 +348,25 @@ void ControllerManager::configure_controller_manager()
   }
 }
 
+void ControllerManager::init_distributed_sub_controller_manager()
+{
+  int64_t distributed_interfaces_publish_period;
+  if (get_parameter("distributed_interfaces_publish_period", distributed_interfaces_publish_period))
+  {
+    distributed_interfaces_publish_period_ =
+      std::chrono::milliseconds(distributed_interfaces_publish_period);
+  }
+  else
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "'distributed_interfaces_publish_period' parameter not set, using default value.");
+  }
+  add_hardware_state_publishers();
+  add_hardware_command_forwarders();
+  register_sub_controller_manager();
+}
+
 void ControllerManager::init_distributed_main_controller_services()
 {
   distributed_system_srv_callback_group_ =
@@ -440,7 +457,8 @@ void ControllerManager::add_hardware_state_publishers()
 {
   std::vector<std::shared_ptr<distributed_control::StatePublisher>> state_publishers_vec;
   state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size());
-  state_publishers_vec = resource_manager_->create_hardware_state_publishers(get_namespace());
+  state_publishers_vec = resource_manager_->create_hardware_state_publishers(
+    get_namespace(), distributed_interfaces_publish_period());
 
   for (auto const & state_publisher : state_publishers_vec)
   {
@@ -461,7 +479,8 @@ void ControllerManager::add_hardware_command_forwarders()
 {
   std::vector<std::shared_ptr<distributed_control::CommandForwarder>> command_forwarder_vec;
   command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size());
-  command_forwarder_vec = resource_manager_->create_hardware_command_forwarders(get_namespace());
+  command_forwarder_vec = resource_manager_->create_hardware_command_forwarders(
+    get_namespace(), distributed_interfaces_publish_period());
 
   for (auto const & command_forwarder : command_forwarder_vec)
   {
@@ -2185,6 +2204,11 @@ std::pair<std::string, std::string> ControllerManager::split_command_interface(
 
 unsigned int ControllerManager::get_update_rate() const { return update_rate_; }
 
+std::chrono::milliseconds ControllerManager::distributed_interfaces_publish_period() const
+{
+  return distributed_interfaces_publish_period_;
+}
+
 void ControllerManager::propagate_deactivation_of_chained_mode(
   const std::vector<ControllerSpec> & controllers)
 {

hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -1,6 +1,7 @@
 #ifndef DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_
 #define DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -22,8 +23,8 @@ class CommandForwarder final
 {
 public:
   explicit CommandForwarder(
-    std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr_,
-    const std::string & ns = "");
+    std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr,
+    std::chrono::milliseconds period_in_ms, const std::string & ns = "");
 
   CommandForwarder() = delete;
 
@@ -53,6 +54,8 @@ class CommandForwarder final
 
   std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr_;
   const std::string namespace_;
+  const std::chrono::milliseconds period_in_ms_;
+
   const std::string topic_name_;
   std::string subscription_topic_name_;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;

hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp

@@ -22,7 +22,7 @@ class StatePublisher final
 public:
   explicit StatePublisher(
     std::unique_ptr<hardware_interface::LoanedStateInterface> loaned_state_interface_ptr,
-    const std::string & ns = "");
+    std::chrono::milliseconds period_in_ms, const std::string & ns = "");
 
   StatePublisher() = delete;
 
@@ -49,6 +49,8 @@ class StatePublisher final
 
   std::unique_ptr<hardware_interface::LoanedStateInterface> loaned_state_interface_ptr_;
   const std::string namespace_;
+  std::chrono::milliseconds period_in_ms_;
+
   const std::string topic_name_;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
   rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr state_value_pub_;

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -131,10 +131,11 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
     std::shared_ptr<distributed_control::SubControllerManagerWrapper> sub_controller_manager);
 
   std::vector<std::shared_ptr<distributed_control::StatePublisher>>
-  create_hardware_state_publishers(const std::string & ns);
+  create_hardware_state_publishers(const std::string & ns, std::chrono::milliseconds update_period);
 
   std::vector<std::shared_ptr<distributed_control::CommandForwarder>>
-  create_hardware_command_forwarders(const std::string & ns);
+  create_hardware_command_forwarders(
+    const std::string & ns, std::chrono::milliseconds update_period);
 
   std::pair<bool, std::shared_ptr<distributed_control::CommandForwarder>> find_command_forwarder(
     const std::string & key);

hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp

@@ -13,8 +13,9 @@ namespace distributed_control
 
 CommandForwarder::CommandForwarder(
   std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr,
-  const std::string & ns)
+  std::chrono::milliseconds period_in_ms, const std::string & ns)
 : loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)),
+  period_in_ms_(period_in_ms),
   namespace_(ns),
   topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state")
 {
@@ -25,8 +26,8 @@ CommandForwarder::CommandForwarder(
 
   state_value_pub_ = node_->create_publisher<std_msgs::msg::Float64>(topic_name_, 10);
   // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then
-  timer_ =
-    node_->create_wall_timer(50ms, std::bind(&CommandForwarder::publish_value_on_timer, this));
+  timer_ = node_->create_wall_timer(
+    period_in_ms_, std::bind(&CommandForwarder::publish_value_on_timer, this));
   RCLCPP_INFO(node_->get_logger(), "Creating CommandForwarder<%s>.", topic_name_.c_str());
 }
 

hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp

@@ -13,8 +13,9 @@ namespace distributed_control
 
 StatePublisher::StatePublisher(
   std::unique_ptr<hardware_interface::LoanedStateInterface> loaned_state_interface_ptr,
-  const std::string & ns)
+  std::chrono::milliseconds period_in_ms, const std::string & ns)
 : loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)),
+  period_in_ms_(period_in_ms),
   namespace_(ns),
   topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state")
 {
@@ -25,7 +26,8 @@ StatePublisher::StatePublisher(
 
   state_value_pub_ = node_->create_publisher<std_msgs::msg::Float64>(topic_name_, 10);
   // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then
-  timer_ = node_->create_wall_timer(50ms, std::bind(&StatePublisher::publish_value_on_timer, this));
+  timer_ = node_->create_wall_timer(
+    period_in_ms_, std::bind(&StatePublisher::publish_value_on_timer, this));
   RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", topic_name_.c_str());
 }
 

hardware_interface/src/resource_manager.cpp

@@ -951,7 +951,8 @@ ResourceManager::import_command_interfaces_of_sub_controller_manager(
 }
 
 std::vector<std::shared_ptr<distributed_control::StatePublisher>>
-ResourceManager::create_hardware_state_publishers(const std::string & ns)
+ResourceManager::create_hardware_state_publishers(
+  const std::string & ns, std::chrono::milliseconds update_period)
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
   std::vector<std::shared_ptr<distributed_control::StatePublisher>> state_publishers_vec;
@@ -962,7 +963,7 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns)
     auto state_publisher = std::make_shared<distributed_control::StatePublisher>(
       std::move(std::make_unique<hardware_interface::LoanedStateInterface>(
         claim_state_interface(state_interface))),
-      ns);
+      update_period, ns);
 
     resource_storage_->add_state_publisher(state_publisher);
     state_publishers_vec.push_back(state_publisher);
@@ -972,7 +973,8 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns)
 }
 
 std::vector<std::shared_ptr<distributed_control::CommandForwarder>>
-ResourceManager::create_hardware_command_forwarders(const std::string & ns)
+ResourceManager::create_hardware_command_forwarders(
+  const std::string & ns, std::chrono::milliseconds update_period)
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
   std::vector<std::shared_ptr<distributed_control::CommandForwarder>> command_forwarders_vec;
@@ -983,7 +985,7 @@ ResourceManager::create_hardware_command_forwarders(const std::string & ns)
     auto command_forwarder = std::make_shared<distributed_control::CommandForwarder>(
       std::move(std::make_unique<hardware_interface::LoanedCommandInterface>(
         claim_command_interface(command_interface))),
-      ns);
+      update_period, ns);
 
     resource_storage_->add_command_forwarder(command_forwarder);
     command_forwarders_vec.push_back(command_forwarder);