add helper for evaluation (qos, publish, publish qos)

Author: mamueluth

controller_manager/include/controller_manager/controller_manager.hpp

@@ -201,6 +201,9 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   unsigned int get_update_rate() const;
 
+  CONTROLLER_MANAGER_PUBLIC
+  rmw_qos_profile_t determine_qos_profile(const std::string & key) const;
+
   CONTROLLER_MANAGER_PUBLIC
   bool is_central_controller_manager() const;
 
@@ -478,6 +481,10 @@ class ControllerManager : public rclcpp::Node
   bool sub_controller_manager_ = false;
   bool central_controller_manager_ = false;
   bool use_multiple_nodes_ = false;
+  std::shared_ptr<evaluation_helper::Evaluation_Helper> qos_helper_;
+  std::string handles_qos_key_ = "system_default";
+  bool publish_evaluation_msg_ = true;
+  std::string evaluation_qos_key_ = "system_default";
   std::vector<std::string> command_interfaces_to_export_ = std::vector<std::string>({});
   std::vector<std::string> state_interfaces_to_export_ = std::vector<std::string>({});
 

controller_manager/src/controller_manager.cpp

@@ -26,6 +26,7 @@
 #include "controller_manager_msgs/msg/publisher_description.hpp"
 
 #include "hardware_interface/distributed_control_interface/command_forwarder.hpp"
+#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp"
 #include "hardware_interface/distributed_control_interface/state_publisher.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -408,6 +409,24 @@ void ControllerManager::get_and_initialize_distributed_parameters()
       get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s",
       use_multiple_nodes_ ? "true" : "false");
   }
+  if (!get_parameter("handles_qos_key", handles_qos_key_))
+  {
+    RCLCPP_WARN(
+      get_logger(), "'handles_qos_key' parameter not set, using default value:%s",
+      handles_qos_key_.c_str());
+  }
+  if (!get_parameter("publish_evaluation_msg", publish_evaluation_msg_))
+  {
+    RCLCPP_WARN(
+      get_logger(), "'publish_evaluation_msg' parameter not set, using default value:%s",
+      publish_evaluation_msg_ ? "true" : "false");
+  }
+  if (!get_parameter("evaluation_qos_key", evaluation_qos_key_))
+  {
+    RCLCPP_WARN(
+      get_logger(), "'evaluation_qos_key' parameter not set, using default value:%s",
+      evaluation_qos_key_.c_str());
+  }
 }
 
 void ControllerManager::configure_controller_manager()
@@ -462,8 +481,42 @@ ControllerManager::controller_manager_type ControllerManager::determine_controll
   return controller_manager_type::unkown_type;
 }
 
+rmw_qos_profile_t ControllerManager::determine_qos_profile(const std::string & key) const
+{
+  if (key == "sensor_data")
+  {
+    return evaluation_helper::rmw_qos_profile_sensor_data;
+  }
+  else if (key == "sensor_data_1")
+  {
+    return evaluation_helper::rmw_qos_profile_sensor_data_1;
+  }
+  else if (key == "sensor_data_100")
+  {
+    return evaluation_helper::rmw_qos_profile_sensor_data_100;
+  }
+  else if (key == "reliable")
+  {
+    return evaluation_helper::rmw_qos_profile_reliable;
+  }
+  else if (key == "reliable_100")
+  {
+    return evaluation_helper::rmw_qos_profile_reliable_100;
+  }
+  else if (key == "system_default")
+  {
+    return evaluation_helper::rmw_qos_profile_system_default;
+  }
+  throw std::runtime_error("Given qos profile not know");
+}
+
 void ControllerManager::init_distributed_sub_controller_manager()
 {
+  // just for evaluation of concept
+  auto handle_qos_profile = determine_qos_profile(handles_qos_key_);
+  auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_);
+  qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance(
+    handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile);
   // if only one node per sub controller manager is used
   if (!use_multiple_nodes())
   {
@@ -510,6 +563,11 @@ void ControllerManager::init_distributed_sub_controller_manager()
 
 void ControllerManager::init_distributed_central_controller_manager()
 {
+  // just for evaluation of concept
+  auto handle_qos_profile = determine_qos_profile(handles_qos_key_);
+  auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_);
+  qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance(
+    handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile);
   if (!use_multiple_nodes())
   {
     rclcpp::NodeOptions node_options;

hardware_interface/CMakeLists.txt

@@ -29,6 +29,7 @@ add_library(hardware_interface SHARED
   src/component_parser.cpp
   src/hardware_interface/distributed_control_interface/command_forwarder.cpp
   src/hardware_interface/distributed_control_interface/state_publisher.cpp
+  src/hardware_interface/distributed_control_interface/evaluation_helper.cpp
   src/resource_manager.cpp
   src/sensor.cpp
   src/system.cpp

hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -6,6 +6,7 @@
 #include <string>
 #include <vector>
 
+#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp"
 #include "hardware_interface/distributed_control_interface/publisher_description.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 

hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp

@@ -0,0 +1,156 @@
+// Copyright 2020 - 2021 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_
+#define DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_
+
+#include <memory>
+#include <mutex>
+
+#include "rmw/qos_profiles.h"
+#include "rmw/types.h"
+
+namespace evaluation_helper
+{
+
+// All profiles used for evaluation, copy them to make sure that exactly those params are used.
+static const rmw_qos_profile_t rmw_qos_profile_sensor_data = {
+  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  5,
+  RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+  RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false};
+
+static const rmw_qos_profile_t rmw_qos_profile_sensor_data_1 = {
+  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  1,
+  RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+  RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false};
+
+static const rmw_qos_profile_t rmw_qos_profile_sensor_data_100 = {
+  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  100,
+  RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+  RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false};
+
+static const rmw_qos_profile_t rmw_qos_profile_reliable = {
+  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  1,
+  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+  RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false};
+
+static const rmw_qos_profile_t rmw_qos_profile_reliable_100 = {
+  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  100,
+  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+  RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false};
+
+static const rmw_qos_profile_t rmw_qos_profile_system_default = {
+  RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT,
+  RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT,
+  RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
+  RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false};
+
+class Evaluation_Helper
+{
+protected:
+  Evaluation_Helper(
+    const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg,
+    const rmw_qos_profile_t & evaluation_qos_profile)
+  : qos_profile_(qos_profile),
+    publish_evaluation_msg_(publish_evaluation_msg),
+    evaluation_qos_profile_(evaluation_qos_profile)
+  {
+  }
+
+public:
+  /**
+  * Evaluation_Helpers should not be cloneable.
+  */
+  Evaluation_Helper(Evaluation_Helper & other) = delete;
+  /**
+  * Evaluation_Helpers should not be assignable.
+  */
+  void operator=(const Evaluation_Helper &) = delete;
+
+  static std::shared_ptr<Evaluation_Helper> create_instance(
+    const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg,
+    const rmw_qos_profile_t & evaluation_qos_profile)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (qos_profile_instance_ == nullptr)
+    {
+      qos_profile_instance_ = std::shared_ptr<Evaluation_Helper>(
+        new Evaluation_Helper(qos_profile, publish_evaluation_msg, evaluation_qos_profile));
+    }
+    return qos_profile_instance_;
+  }
+
+  static std::shared_ptr<Evaluation_Helper> get_instance()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (qos_profile_instance_ == nullptr)
+    {
+      throw std::runtime_error("Evaluation_Helper not initialized!");
+    }
+    return qos_profile_instance_;
+  }
+
+  rmw_qos_profile_t get_qos_profile() const { return qos_profile_; }
+
+  rmw_qos_profile_t get_evaluation_qos_profile() const { return evaluation_qos_profile_; }
+
+  bool publish_evaluation_msg() const { return publish_evaluation_msg_; }
+
+protected:
+  static std::shared_ptr<Evaluation_Helper> qos_profile_instance_;
+  static std::mutex mutex_;
+
+  const rmw_qos_profile_t qos_profile_;
+  const bool publish_evaluation_msg_;
+  const rmw_qos_profile_t evaluation_qos_profile_;
+};
+
+}  // namespace evaluation_helper
+
+#endif  // DISTRIBUTED_CONTROL__EVALUATION_HELPER_HPP_

hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp

@@ -5,6 +5,7 @@
 #include <string>
 #include <vector>
 
+#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 
 #include "controller_manager_msgs/msg/publisher_description.hpp"

hardware_interface/include/hardware_interface/handle.hpp

@@ -18,6 +18,7 @@
 #include <string>
 #include <utility>
 
+#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp"
 #include "hardware_interface/distributed_control_interface/publisher_description.hpp"
 #include "hardware_interface/macros.hpp"
 #include "hardware_interface/visibility_control.h"
@@ -206,9 +207,13 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle
         get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options,
         false);
     }
+
+    auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance();
+    rclcpp::QoS qos_profile(
+      rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile()));
     // subscribe to topic provided by StatePublisher
     state_value_sub_ = node_->create_subscription<std_msgs::msg::Float64>(
-      get_value_topic_name_, 10,
+      get_value_topic_name_, qos_profile,
       std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1));
   }
 
@@ -354,14 +359,17 @@ class DistributedReadWriteHandle : public ReadWriteHandle
         node_options, false);
     }
 
+    auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance();
+    rclcpp::QoS qos_profile(
+      rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile()));
     // subscribe to topic provided by CommandForwarder
     command_value_sub_ = node_->create_subscription<std_msgs::msg::Float64>(
-      get_value_topic_name_, 10,
+      get_value_topic_name_, qos_profile,
       std::bind(&DistributedReadWriteHandle::get_value_cb, this, std::placeholders::_1));
 
     // create publisher so that we can forward the commands
     command_value_pub_ =
-      node_->create_publisher<std_msgs::msg::Float64>(forward_command_topic_name_, 10);
+      node_->create_publisher<std_msgs::msg::Float64>(forward_command_topic_name_, qos_profile);
   }
 
   explicit DistributedReadWriteHandle(const std::string & interface_name)

hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp

@@ -32,7 +32,10 @@ CommandForwarder::CommandForwarder(
       namespace_, node_options, false);
   }
 
-  state_value_pub_ = node_->create_publisher<std_msgs::msg::Float64>(topic_name_, 10);
+  auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance();
+  rclcpp::QoS qos_profile(
+    rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile()));
+  state_value_pub_ = node_->create_publisher<std_msgs::msg::Float64>(topic_name_, qos_profile);
   // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then
   timer_ = node_->create_wall_timer(
     period_in_ms_, std::bind(&CommandForwarder::publish_value_on_timer, this));
@@ -89,9 +92,12 @@ CommandForwarder::create_publisher_description_msg() const
 
 void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name)
 {
+  auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance();
+  rclcpp::QoS qos_profile(
+    rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile()));
   subscription_topic_name_ = topic_name;
   command_subscription_ = node_->create_subscription<std_msgs::msg::Float64>(
-    subscription_topic_name_, 10,
+    subscription_topic_name_, qos_profile,
     std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1));
 }
 

hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp

@@ -0,0 +1,5 @@
+#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp"
+
+std::shared_ptr<evaluation_helper::Evaluation_Helper>
+  evaluation_helper::Evaluation_Helper::qos_profile_instance_{nullptr};
+std::mutex evaluation_helper::Evaluation_Helper::mutex_;

hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp

@@ -32,7 +32,10 @@ StatePublisher::StatePublisher(
       node_options, false);
   }
 
-  state_value_pub_ = node_->create_publisher<std_msgs::msg::Float64>(topic_name_, 10);
+  auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance();
+  rclcpp::QoS qos_profile(
+    rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile()));
+  state_value_pub_ = node_->create_publisher<std_msgs::msg::Float64>(topic_name_, qos_profile);
   // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then
   timer_ = node_->create_wall_timer(
     period_in_ms_, std::bind(&StatePublisher::publish_value_on_timer, this));