first rough adaption of published massage with header

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,26 @@ 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_;
+ bool publish_evaluation_msg_;
+ rclcpp::Time receive_time_;
 };
 
 } // 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"
@@ -213,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));
  }
@@ -257,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_ << "].");
@@ -269,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_;
 };
 
@@ -365,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)
@@ -412,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(
@@ -437,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,7 +21,9 @@ 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())
@@ -36,7 +38,24 @@ 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);
+
+ 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(
  period_in_ms_, std::bind(&CommandForwarder::publish_value_on_timer, this));
@@ -97,15 +116,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();
@@ -115,16 +134,35 @@ 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)
 {
+ // 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);
+
+ 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

hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp

@@ -34,9 +34,11 @@ StatePublisher::StatePublisher(
  }
 
  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));
@@ -92,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();
@@ -104,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));
 }