From 798f77c7a5b4f7358e53ff23e92f20545763096a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sat, 2 Mar 2024 20:15:53 +0000 Subject: [PATCH] Example 14: Check for NaN in write method Since activation of the "hardware" takes some time, the resource manager might try to write the initial NaN value to the hardware. This commit adds a check to the write method for that. --- .../rrbot_actuator_without_feedback.cpp | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 3c7a6b887..f45344a94 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -183,21 +183,23 @@ hardware_interface::return_type RRBotActuatorWithoutFeedback::read( hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWithoutFeedback::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // START: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", hw_joint_command_); - - // Simulate sending commands to the hardware - std::ostringstream data; - data << hw_joint_command_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Sending data command: %s", - data.str().c_str()); - send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0); + if (!std::isnan(hw_joint_command_)) + { + // START: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", hw_joint_command_); - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Joints successfully written!"); - // END: This part here is for exemplary purposes - Please do not copy to your production code + // Simulate sending commands to the hardware + std::ostringstream data; + data << hw_joint_command_; + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Sending data command: %s", + data.str().c_str()); + send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0); + RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Joints successfully written!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + } return hardware_interface::return_type::OK; }