first rough adaption of published massage with header

Author: mamueluth

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,4 @@
+string identifier
+string type
+uint32 seq
+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/state_publisher.hpp

@@ -9,10 +9,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
 {
@@ -53,8 +53,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

@@ -22,6 +22,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"
@@ -207,7 +208,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle
         false);
     }
     // 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_, 10,
       std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1));
   }
@@ -251,7 +252,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_ << "].");
@@ -263,7 +264,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_;
 };
 

hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp

@@ -32,7 +32,8 @@ StatePublisher::StatePublisher(
       node_options, false);
   }
 
-  state_value_pub_ = node_->create_publisher<std_msgs::msg::Float64>(topic_name_, 10);
+  state_value_pub_ =
+    node_->create_publisher<controller_manager_msgs::msg::InterfaceData>(topic_name_, 10);
   // 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));
@@ -88,7 +89,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();
@@ -104,6 +105,8 @@ void StatePublisher::publish_value_on_timer()
 
   // 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));
 }