diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index cd51c6ab22..3630ae0776 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/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 diff --git a/controller_manager_msgs/msg/Evaluation.msg b/controller_manager_msgs/msg/Evaluation.msg new file mode 100644 index 0000000000..9147146a08 --- /dev/null +++ b/controller_manager_msgs/msg/Evaluation.msg @@ -0,0 +1,5 @@ +string identifier +string type +uint32 seq +uint64 receive_time +builtin_interfaces/Time receive_stamp \ No newline at end of file diff --git a/controller_manager_msgs/msg/Header.msg b/controller_manager_msgs/msg/Header.msg new file mode 100644 index 0000000000..71b6028182 --- /dev/null +++ b/controller_manager_msgs/msg/Header.msg @@ -0,0 +1 @@ +uint32 seq diff --git a/controller_manager_msgs/msg/InterfaceData.msg b/controller_manager_msgs/msg/InterfaceData.msg new file mode 100644 index 0000000000..54129ad221 --- /dev/null +++ b/controller_manager_msgs/msg/InterfaceData.msg @@ -0,0 +1,2 @@ +controller_manager_msgs/Header header +float64 data \ No newline at end of file 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 fb94a09450..d2745418bf 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 @@ -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 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 node_; - rclcpp::Publisher::SharedPtr state_value_pub_; - rclcpp::Subscription::SharedPtr command_subscription_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::Subscription::SharedPtr + command_subscription_; rclcpp::TimerBase::SharedPtr timer_; + std::shared_ptr evaluation_node_; + 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/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp index 65a166658a..42a75aa48b 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/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 node_; - rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::Publisher::SharedPtr state_value_pub_; rclcpp::TimerBase::SharedPtr timer_; + uint32_t seq_number_ = 0; }; } // namespace distributed_control diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 0b4aaba12d..e2d7e7e571 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/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( + state_value_sub_ = node_->create_subscription( 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::SharedPtr state_value_sub_; + rclcpp::Subscription::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( + command_value_sub_ = node_->create_subscription( 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(forward_command_topic_name_, qos_profile); + command_value_pub_ = node_->create_publisher( + forward_command_topic_name_, qos_profile); } explicit DistributedReadWriteHandle(const std::string & interface_name) @@ -412,11 +413,12 @@ class DistributedReadWriteHandle : public ReadWriteHandle void set_value(double value) override { - auto msg = std::make_unique(); + auto msg = std::make_unique(); 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)); } @@ -424,7 +426,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle 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::SharedPtr command_value_sub_; + rclcpp::Subscription::SharedPtr command_value_sub_; std::string forward_command_topic_name_; - rclcpp::Publisher::SharedPtr command_value_pub_; + rclcpp::Publisher::SharedPtr command_value_pub_; double value_; + uint_fast32_t seq_number_ = 0; }; class DistributedCommandInterface : public DistributedReadWriteHandle 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 203be40c00..9d2180ba19 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 @@ -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(topic_name_, qos_profile); + state_value_pub_ = + node_->create_publisher(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( + 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( period_in_ms_, std::bind(&CommandForwarder::publish_value_on_timer, this)); @@ -97,7 +116,7 @@ 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( + command_subscription_ = node_->create_subscription( subscription_topic_name_, qos_profile, std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1)); } @@ -105,7 +124,7 @@ void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_ void CommandForwarder::publish_value_on_timer() { // Todo(Manuel) create custom msg and return success or failure not just nan. - auto msg = std::make_unique(); + auto msg = std::make_unique(); try { msg->data = loaned_command_interface_ptr_->get_value(); @@ -115,16 +134,35 @@ void CommandForwarder::publish_value_on_timer() msg->data = std::numeric_limits::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(); + 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 diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index bec3564d89..4ce75a05c3 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/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(topic_name_, qos_profile); + state_value_pub_ = + node_->create_publisher(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(); + auto msg = std::make_unique(); try { msg->data = loaned_state_interface_ptr_->get_value(); @@ -104,10 +106,11 @@ void StatePublisher::publish_value_on_timer() msg->data = std::numeric_limits::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)); }