diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp index 4f2e0c85cb7..d2745418bf0 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -71,6 +71,8 @@ class CommandForwarder final rclcpp::Publisher::SharedPtr evaluation_pub_; const std::string evaluation_type_ = "commandInterface"; std::string evaluation_identifier_; + bool publish_evaluation_msg_; + rclcpp::Time receive_time_; }; } // namespace distributed_control 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 abc762cca1d..9d2180ba192 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 @@ -41,14 +41,20 @@ CommandForwarder::CommandForwarder( state_value_pub_ = node_->create_publisher(topic_name_, qos_profile); - rclcpp::NodeOptions node_options; - node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); - evaluation_node_ = std::make_shared( - loaned_command_interface_ptr_->get_underscore_separated_name() + "evaluation_node", namespace_, - node_options, false); - evaluation_pub_ = evaluation_node_->create_publisher( - evaluation_topic_name_, 10); - evaluation_identifier_ = loaned_command_interface_ptr_->get_underscore_separated_name(); + 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( + 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( + 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( @@ -136,20 +142,27 @@ void CommandForwarder::publish_value_on_timer() void CommandForwarder::forward_command(const controller_manager_msgs::msg::InterfaceData & msg) { - auto receive_time = evaluation_node_->now(); + // 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); - auto evaluation_msg = std::make_unique(); - 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; - 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)); + if (publish_evaluation_msg_) + { + auto evaluation_msg = std::make_unique(); + 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; + 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 \ No newline at end of file