Distributed control aio value change new

controller_interface/CMakeLists.txt

@@ -30,6 +30,7 @@ ament_target_dependencies(controller_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEP
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL")
 
+set(BUILD_TESTING 0)
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(sensor_msgs REQUIRED)

controller_manager/CMakeLists.txt

@@ -43,6 +43,7 @@ target_link_libraries(ros2_control_node PRIVATE
   controller_manager
 )
 
+set(BUILD_TESTING 0)
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)

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;
 
@@ -210,9 +213,6 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   bool use_multiple_nodes() const;
 
-  CONTROLLER_MANAGER_PUBLIC
-  std::chrono::milliseconds distributed_interfaces_publish_period() const;
-
 protected:
   CONTROLLER_MANAGER_PUBLIC
   void init_services();
@@ -478,6 +478,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>({});
 
@@ -486,7 +490,6 @@ class ControllerManager : public rclcpp::Node
   // associated with it ... (create on distributed Handles and StatePublisher/CommandForwarder)
   // needs to be checked if is nullptr before usage
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> distributed_pub_sub_node_ = nullptr;
-  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

@@ -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"
@@ -48,10 +49,6 @@ rclcpp::QoS qos_services =
     .reliable()
     .durability_volatile();
 
-rclcpp::QoSInitialization qos_profile_services_keep_all_persist_init(
-  RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1);
-rclcpp::QoS qos_profile_services_keep_all(qos_profile_services_keep_all_persist_init);
-
 inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
 {
   return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
@@ -366,19 +363,6 @@ void ControllerManager::get_and_initialize_distributed_parameters()
       central_controller_manager_ ? "true" : "false");
   }
 
-  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.");
-  }
-
   if (!get_parameter("export_command_interfaces", command_interfaces_to_export_))
   {
     RCLCPP_WARN(
@@ -408,6 +392,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,13 +464,48 @@ 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())
   {
     // create node for publishing/subscribing
     rclcpp::NodeOptions node_options;
+    node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
     distributed_pub_sub_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
       std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false);
     //try to add to executor
@@ -510,9 +547,15 @@ 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;
+    node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
     distributed_pub_sub_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
       std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false);
     //try to add to executor
@@ -533,6 +576,11 @@ void ControllerManager::init_distributed_central_controller_manager()
 
 void ControllerManager::init_distributed_central_controller_manager_services()
 {
+  rclcpp::QoS qos_distributed_services_keep_10 =
+    rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 10))
+      .reliable()
+      .durability_volatile();
+
   distributed_system_srv_callback_group_ =
     create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
@@ -542,15 +590,15 @@ void ControllerManager::init_distributed_central_controller_manager_services()
       std::bind(
         &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1,
         std::placeholders::_2),
-      qos_profile_services_keep_all, distributed_system_srv_callback_group_);
+      qos_distributed_services_keep_10, distributed_system_srv_callback_group_);
 
   register_sub_controller_manager_references_srv_ =
     create_service<controller_manager_msgs::srv::RegisterSubControllerManagerReferences>(
       "register_sub_controller_manager_references",
       std::bind(
         &ControllerManager::register_sub_controller_manager_references_srv_cb, this,
         std::placeholders::_1, std::placeholders::_2),
-      qos_profile_services_keep_all, distributed_system_srv_callback_group_);
+      qos_distributed_services_keep_10, distributed_system_srv_callback_group_);
 }
 
 void ControllerManager::register_sub_controller_manager_srv_cb(
@@ -708,7 +756,7 @@ void ControllerManager::create_hardware_state_publishers(
       state_publisher = std::make_shared<distributed_control::StatePublisher>(
         std::move(std::make_unique<hardware_interface::LoanedStateInterface>(
           resource_manager_->claim_state_interface(state_interface))),
-        get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
+        get_namespace(), distributed_pub_sub_node_);
     }
     catch (const std::exception & e)
     {
@@ -747,7 +795,7 @@ void ControllerManager::create_hardware_command_forwarders(
       command_forwarder = std::make_shared<distributed_control::CommandForwarder>(
         std::move(std::make_unique<hardware_interface::LoanedCommandInterface>(
           resource_manager_->claim_command_interface(command_interface))),
-        get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
+        get_namespace(), distributed_pub_sub_node_);
     }
     catch (const std::exception & e)
     {
@@ -2622,11 +2670,6 @@ bool ControllerManager::is_sub_controller_manager() const { return sub_controlle
 
 bool ControllerManager::use_multiple_nodes() const { return use_multiple_nodes_; }
 
-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)
 {

controller_manager_msgs/CMakeLists.txt

@@ -9,9 +9,12 @@ find_package(rosidl_default_generators REQUIRED)
 set(msg_files
   msg/ControllerState.msg
   msg/ChainConnection.msg
+  msg/Evaluation.msg
   msg/HandleName.msg
   msg/HardwareComponentState.msg
   msg/HardwareInterface.msg
+  msg/Header.msg
+  msg/InterfaceData.msg
   msg/PublisherDescription.msg
 )
 set(srv_files

controller_manager_msgs/msg/Evaluation.msg

@@ -0,0 +1,5 @@
+string identifier
+string type
+uint32 seq
+uint64 receive_time
+builtin_interfaces/Time receive_stamp

controller_manager_msgs/msg/Header.msg

@@ -0,0 +1 @@
+uint32 seq

controller_manager_msgs/msg/InterfaceData.msg

@@ -0,0 +1,2 @@
+controller_manager_msgs/Header header
+float64 data

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
@@ -76,6 +77,7 @@ target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_
 pluginlib_export_plugin_description_file(
   hardware_interface fake_components_plugin_description.xml)
 
+set(BUILD_TESTING 0)
 if(BUILD_TESTING)
 
   find_package(ament_cmake_gmock REQUIRED)

hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -6,15 +6,17 @@
 #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"
 
 #include "controller_manager_msgs/msg/publisher_description.hpp"
 
+#include "controller_manager_msgs/msg/evaluation.hpp"
+#include "controller_manager_msgs/msg/interface_data.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "std_msgs/msg/float64.hpp"
 
 namespace distributed_control
 {
@@ -24,8 +26,7 @@ class CommandForwarder final
 public:
   explicit CommandForwarder(
     std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr,
-    const std::string & ns, std::chrono::milliseconds period_in_ms,
-    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
+    const std::string & ns, std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
 
   CommandForwarder() = delete;
 
@@ -50,19 +51,24 @@ class CommandForwarder final
   void subscribe_to_command_publisher(const std::string & topic_name);
 
 private:
-  void publish_value_on_timer();
-  void forward_command(const std_msgs::msg::Float64 & msg);
+  void forward_command(const controller_manager_msgs::msg::InterfaceData & msg);
 
   std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr_;
   const std::string namespace_;
-  const std::chrono::milliseconds period_in_ms_;
-
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+  const std::string node_name_;
   const std::string topic_name_;
+  const std::string evaluation_topic_name_;
   std::string subscription_topic_name_;
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
-  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr state_value_pub_;
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr command_subscription_;
-  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<controller_manager_msgs::msg::InterfaceData>::SharedPtr state_value_pub_;
+  rclcpp::Subscription<controller_manager_msgs::msg::InterfaceData>::SharedPtr
+    command_subscription_;
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> evaluation_node_;
+  rclcpp::Publisher<controller_manager_msgs::msg::Evaluation>::SharedPtr evaluation_pub_;
+  const std::string evaluation_type_ = "commandInterface";
+  std::string evaluation_identifier_;
+  bool publish_evaluation_msg_;
+  rclcpp::Time receive_time_;
 };
 
 }  // namespace distributed_control