add time and publishing of when command is received

Author: mamueluth

controller_manager/src/controller_manager.cpp

@@ -469,6 +469,7 @@ void ControllerManager::init_distributed_sub_controller_manager()
   {
     // 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
@@ -513,6 +514,7 @@ void ControllerManager::init_distributed_central_controller_manager()
   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

controller_manager_msgs/msg/Evaluation.msg

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

hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -11,6 +11,7 @@
 
 #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"
@@ -58,12 +59,17 @@ class CommandForwarder final
   const std::chrono::milliseconds period_in_ms_;
 
   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<controller_manager_msgs::msg::InterfaceData>::SharedPtr state_value_pub_;
   rclcpp::Subscription<controller_manager_msgs::msg::InterfaceData>::SharedPtr
     command_subscription_;
   rclcpp::TimerBase::SharedPtr timer_;
+  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_;
 };
 
 }  // namespace distributed_control

hardware_interface/include/hardware_interface/handle.hpp

@@ -203,6 +203,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle
     if (!node_.get())
     {
       rclcpp::NodeOptions node_options;
+      node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
       node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
         get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options,
         false);
@@ -350,6 +351,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle
     if (!node_.get())
     {
       rclcpp::NodeOptions node_options;
+      node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
       node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
         get_underscore_separated_name() + "_distributed_command_interface", namespace_,
         node_options, false);

hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp

@@ -21,19 +21,32 @@ CommandForwarder::CommandForwarder(
   namespace_(ns),
   period_in_ms_(period_in_ms),
   node_(node),
-  topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state")
+  topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state"),
+  evaluation_topic_name_(
+    loaned_command_interface_ptr_->get_underscore_separated_name() + "_EVALUATION")
 {
   // if we did not get a node passed, we create one ourselves
   if (!node_.get())
   {
     rclcpp::NodeOptions node_options;
+    node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
     node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
       loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder",
       namespace_, node_options, false);
   }
 
   state_value_pub_ =
     node_->create_publisher<controller_manager_msgs::msg::InterfaceData>(topic_name_, 10);
+
+  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();
+
   // 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));
@@ -117,6 +130,17 @@ void CommandForwarder::publish_value_on_timer()
 
 void CommandForwarder::forward_command(const controller_manager_msgs::msg::InterfaceData & msg)
 {
+  auto evaluation_msg = std::make_unique<controller_manager_msgs::msg::Evaluation>();
+  evaluation_msg->receive_stamp = evaluation_node_->now();
+  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));
+
   loaned_command_interface_ptr_->set_value(msg.data);
 }
 

hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp

@@ -27,6 +27,7 @@ StatePublisher::StatePublisher(
   if (!node_.get())
   {
     rclcpp::NodeOptions node_options;
+    node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME);
     node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
       loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_,
       node_options, false);