From b2f8f91ffd3128966861eaad68655d9067682dc7 Mon Sep 17 00:00:00 2001
From: Manuel M <mamueluth@gmail.com>
Date: Sun, 11 Jun 2023 15:09:27 +0200
Subject: [PATCH] add helper for evaluation (qos, publish, publish qos)

---
 .../command_forwarder.hpp                     |  2 +
 .../command_forwarder.cpp                     | 51 ++++++++++++-------
 2 files changed, 34 insertions(+), 19 deletions(-)

diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp
index 4f2e0c85cb..d2745418bf 100644
--- a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp
+++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp
@@ -71,6 +71,8 @@ class CommandForwarder final
   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
diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp
index abc762cca1..9d2180ba19 100644
--- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp
+++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp
@@ -41,14 +41,20 @@ CommandForwarder::CommandForwarder(
   state_value_pub_ =
     node_->create_publisher<controller_manager_msgs::msg::InterfaceData>(topic_name_, qos_profile);
 
-  rclcpp::NodeOptions node_options;
-  node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
-  evaluation_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
-    loaned_command_interface_ptr_->get_underscore_separated_name() + "evaluation_node", namespace_,
-    node_options, false);
-  evaluation_pub_ = evaluation_node_->create_publisher<controller_manager_msgs::msg::Evaluation>(
-    evaluation_topic_name_, 10);
-  evaluation_identifier_ = loaned_command_interface_ptr_->get_underscore_separated_name();
+  publish_evaluation_msg_ = evaluation_helper->publish_evaluation_msg();
+  if (publish_evaluation_msg_)
+  {
+    rclcpp::NodeOptions node_options;
+    node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
+    evaluation_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
+      loaned_command_interface_ptr_->get_underscore_separated_name() + "evaluation_node",
+      namespace_, node_options, false);
+    rclcpp::QoS evaluation_qos_profile(
+      rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_evaluation_qos_profile()));
+    evaluation_pub_ = evaluation_node_->create_publisher<controller_manager_msgs::msg::Evaluation>(
+      evaluation_topic_name_, evaluation_qos_profile);
+    evaluation_identifier_ = loaned_command_interface_ptr_->get_underscore_separated_name();
+  }
 
   // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then
   timer_ = node_->create_wall_timer(
@@ -136,20 +142,27 @@ void CommandForwarder::publish_value_on_timer()
 
 void CommandForwarder::forward_command(const controller_manager_msgs::msg::InterfaceData & msg)
 {
-  auto receive_time = evaluation_node_->now();
+  // first get timestamp to be as precise as possible
+  if (publish_evaluation_msg_)
+  {
+    receive_time_ = evaluation_node_->now();
+  }
   //set value before publishing
   loaned_command_interface_ptr_->set_value(msg.data);
 
-  auto evaluation_msg = std::make_unique<controller_manager_msgs::msg::Evaluation>();
-  evaluation_msg->receive_stamp = receive_time;
-  evaluation_msg->receive_time =
-    static_cast<uint64_t>(evaluation_msg->receive_stamp.sec) * 1'000'000'000ULL +
-    evaluation_msg->receive_stamp.nanosec;
-  evaluation_msg->type = evaluation_type_;
-  evaluation_msg->identifier = evaluation_identifier_;
-  evaluation_msg->seq = msg.header.seq;
-  // todo check for QoS to publish immediately and never block to be fast as possible
-  evaluation_pub_->publish(std::move(evaluation_msg));
+  if (publish_evaluation_msg_)
+  {
+    auto evaluation_msg = std::make_unique<controller_manager_msgs::msg::Evaluation>();
+    evaluation_msg->receive_stamp = receive_time_;
+    evaluation_msg->receive_time =
+      static_cast<uint64_t>(evaluation_msg->receive_stamp.sec) * 1'000'000'000ULL +
+      evaluation_msg->receive_stamp.nanosec;
+    evaluation_msg->type = evaluation_type_;
+    evaluation_msg->identifier = evaluation_identifier_;
+    evaluation_msg->seq = msg.header.seq;
+    // todo check for QoS to publish immediately and never block to be fast as possible
+    evaluation_pub_->publish(std::move(evaluation_msg));
+  }
 }
 
 }  // namespace distributed_control
\ No newline at end of file