From a7f0925a29c7344c6fb754129c565452e98e463b Mon Sep 17 00:00:00 2001 From: Manuel M Date: Sat, 10 Jun 2023 14:49:54 +0200 Subject: [PATCH] forward value before publishing timestamp --- .../distributed_control_interface/command_forwarder.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index 114af457f7..abc762cca1 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -136,8 +136,12 @@ void CommandForwarder::publish_value_on_timer() void CommandForwarder::forward_command(const controller_manager_msgs::msg::InterfaceData & msg) { + auto receive_time = evaluation_node_->now(); + //set value before publishing + loaned_command_interface_ptr_->set_value(msg.data); + auto evaluation_msg = std::make_unique(); - evaluation_msg->receive_stamp = evaluation_node_->now(); + evaluation_msg->receive_stamp = receive_time; evaluation_msg->receive_time = static_cast(evaluation_msg->receive_stamp.sec) * 1'000'000'000ULL + evaluation_msg->receive_stamp.nanosec; @@ -146,8 +150,6 @@ void CommandForwarder::forward_command(const controller_manager_msgs::msg::Inter 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); } } // namespace distributed_control \ No newline at end of file