first rough adaption of published massage with header

changes for command side, publishing missing

add time and publishing of when command is received

Author: mamueluth

controller_manager/src/controller_manager.cpp

@@ -518,6 +518,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
@@ -567,6 +568,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/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/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -12,10 +12,11 @@
 
 #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
 {
@@ -52,18 +53,24 @@ class CommandForwarder final
 
 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_;
 
   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::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/distributed_control_interface/state_publisher.hpp

@@ -10,10 +10,10 @@
 
 #include "controller_manager_msgs/msg/publisher_description.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
 {
@@ -54,8 +54,9 @@ class StatePublisher final
 
   const std::string topic_name_;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
-  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr state_value_pub_;
+  rclcpp::Publisher<controller_manager_msgs::msg::InterfaceData>::SharedPtr state_value_pub_;
   rclcpp::TimerBase::SharedPtr timer_;
+  uint32_t seq_number_ = 0;
 };
 
 }  // namespace distributed_control

hardware_interface/include/hardware_interface/handle.hpp

@@ -23,6 +23,7 @@
 #include "hardware_interface/macros.hpp"
 #include "hardware_interface/visibility_control.h"
 
+#include "controller_manager_msgs/msg/interface_data.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "std_msgs/msg/float64.hpp"
@@ -203,6 +204,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);
@@ -212,7 +214,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle
     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>(
+    state_value_sub_ = node_->create_subscription<controller_manager_msgs::msg::InterfaceData>(
       get_value_topic_name_, qos_profile,
       std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1));
   }
@@ -256,7 +258,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle
   }
 
 protected:
-  void get_value_cb(const std_msgs::msg::Float64 & msg)
+  void get_value_cb(const controller_manager_msgs::msg::InterfaceData & msg)
   {
     value_ = msg.data;
     RCLCPP_DEBUG_STREAM(node_->get_logger(), "Receiving:[" << value_ << "].");
@@ -268,7 +270,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle
   // the namespace the actual StateInterface we subscribe to is in.
   // We need this to create unique names for the StateInterface.
   std::string interface_namespace_;
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr state_value_sub_;
+  rclcpp::Subscription<controller_manager_msgs::msg::InterfaceData>::SharedPtr state_value_sub_;
   double value_;
 };
 
@@ -354,6 +356,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);
@@ -363,13 +366,13 @@ class DistributedReadWriteHandle : public ReadWriteHandle
     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>(
+    command_value_sub_ = node_->create_subscription<controller_manager_msgs::msg::InterfaceData>(
       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_, qos_profile);
+    command_value_pub_ = node_->create_publisher<controller_manager_msgs::msg::InterfaceData>(
+      forward_command_topic_name_, qos_profile);
   }
 
   explicit DistributedReadWriteHandle(const std::string & interface_name)
@@ -410,19 +413,20 @@ class DistributedReadWriteHandle : public ReadWriteHandle
 
   void set_value(double value) override
   {
-    auto msg = std::make_unique<std_msgs::msg::Float64>();
+    auto msg = std::make_unique<controller_manager_msgs::msg::InterfaceData>();
     msg->data = value;
+    msg->header.seq = seq_number_;
+    ++seq_number_;
 
     RCLCPP_DEBUG(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data);
-    std::flush(std::cout);
 
     command_value_pub_->publish(std::move(msg));
   }
 
   std::string forward_command_topic_name() const { return forward_command_topic_name_; }
 
 protected:
-  void get_value_cb(const std_msgs::msg::Float64 & msg)
+  void get_value_cb(const controller_manager_msgs::msg::InterfaceData & msg)
   {
     value_ = msg.data;
     RCLCPP_DEBUG_STREAM(
@@ -435,10 +439,11 @@ class DistributedReadWriteHandle : public ReadWriteHandle
   // the namespace the actual CommandInterface we subscribe to is in.
   // We need this to create unique names for the CommandInterface.
   std::string interface_namespace_;
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr command_value_sub_;
+  rclcpp::Subscription<controller_manager_msgs::msg::InterfaceData>::SharedPtr command_value_sub_;
   std::string forward_command_topic_name_;
-  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr command_value_pub_;
+  rclcpp::Publisher<controller_manager_msgs::msg::InterfaceData>::SharedPtr command_value_pub_;
   double value_;
+  uint_fast32_t seq_number_ = 0;
 };
 
 class DistributedCommandInterface : public DistributedReadWriteHandle

hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp

@@ -21,12 +21,15 @@ 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);
@@ -35,7 +38,18 @@ CommandForwarder::CommandForwarder(
   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);
+  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();
+
   // 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));
@@ -96,15 +110,15 @@ void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_
   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>(
+  command_subscription_ = node_->create_subscription<controller_manager_msgs::msg::InterfaceData>(
     subscription_topic_name_, qos_profile,
     std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1));
 }
 
 void CommandForwarder::publish_value_on_timer()
 {
   // Todo(Manuel) create custom msg and return success or failure not just nan.
-  auto msg = std::make_unique<std_msgs::msg::Float64>();
+  auto msg = std::make_unique<controller_manager_msgs::msg::InterfaceData>();
   try
   {
     msg->data = loaned_command_interface_ptr_->get_value();
@@ -114,15 +128,25 @@ void CommandForwarder::publish_value_on_timer()
     msg->data = std::numeric_limits<double>::quiet_NaN();
   }
   RCLCPP_DEBUG(node_->get_logger(), "Publishing: '%.7lf'", msg->data);
-  std::flush(std::cout);
 
   // Put the message into a queue to be processed by the middleware.
   // This call is non-blocking.
   state_value_pub_->publish(std::move(msg));
 }
 
-void CommandForwarder::forward_command(const std_msgs::msg::Float64 & msg)
+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,15 +27,18 @@ 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);
   }
 
   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);
+  state_value_pub_ =
+    node_->create_publisher<controller_manager_msgs::msg::InterfaceData>(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));
@@ -91,7 +94,7 @@ StatePublisher::create_publisher_description_msg() const
 
 void StatePublisher::publish_value_on_timer()
 {
-  auto msg = std::make_unique<std_msgs::msg::Float64>();
+  auto msg = std::make_unique<controller_manager_msgs::msg::InterfaceData>();
   try
   {
     msg->data = loaned_state_interface_ptr_->get_value();
@@ -103,10 +106,11 @@ void StatePublisher::publish_value_on_timer()
     msg->data = std::numeric_limits<double>::quiet_NaN();
   }
   RCLCPP_DEBUG(node_->get_logger(), "Publishing: '%.7lf'", msg->data);
-  std::flush(std::cout);
 
   // Put the message into a queue to be processed by the middleware.
   // This call is non-blocking.
+  msg->header.seq = seq_number_;
+  ++seq_number_;
   state_value_pub_->publish(std::move(msg));
 }