changes for command side, publishing missing

Author: mamueluth

hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -11,10 +11,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
 {
@@ -51,7 +51,7 @@ 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_;
@@ -60,8 +60,9 @@ class CommandForwarder final
   const std::string 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_;
 };
 

hardware_interface/include/hardware_interface/handle.hpp

@@ -356,13 +356,13 @@ class DistributedReadWriteHandle : public ReadWriteHandle
     }
 
     // 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_, 10,
       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_, 10);
+    command_value_pub_ = node_->create_publisher<controller_manager_msgs::msg::InterfaceData>(
+      forward_command_topic_name_, 10);
   }
 
   explicit DistributedReadWriteHandle(const std::string & interface_name)
@@ -403,19 +403,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(
@@ -428,10 +429,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

@@ -32,7 +32,8 @@ CommandForwarder::CommandForwarder(
       namespace_, 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(&CommandForwarder::publish_value_on_timer, this));
@@ -90,15 +91,15 @@ CommandForwarder::create_publisher_description_msg() const
 void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name)
 {
   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_, 10,
     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();
@@ -108,14 +109,13 @@ 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)
 {
   loaned_command_interface_ptr_->set_value(msg.data);
 }

hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp

@@ -101,7 +101,6 @@ 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.