@@ -217,23 +217,21 @@ hardware_interface::return_type RRBotActuatorWithoutFeedback::read(
217217hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWithoutFeedback::write (
218218 const rclcpp::Time & /* time*/ , const rclcpp::Duration & /* period*/ )
219219{
220- if (std::isfinite (hw_joint_command_))
221- {
222- // START: This part here is for exemplary purposes - Please do not copy to your production code
223- RCLCPP_INFO (
224- rclcpp::get_logger (" RRBotActuatorWithoutFeedback" ), " Writing command: %f" , hw_joint_command_);
220+ // START: This part here is for exemplary purposes - Please do not copy to your production code
221+ RCLCPP_INFO (
222+ rclcpp::get_logger (" RRBotActuatorWithoutFeedback" ), " Writing command: %f" , hw_joint_command_);
225223
226- // Simulate sending commands to the hardware
227- std::ostringstream data;
228- data << hw_joint_command_;
229- RCLCPP_INFO (
230- rclcpp::get_logger (" RRBotActuatorWithoutFeedback" ), " Sending data command: %s" ,
231- data.str ().c_str ());
232- send (sock_, data.str ().c_str (), strlen (data.str ().c_str ()), 0 );
224+ // Simulate sending commands to the hardware
225+ std::ostringstream data;
226+ data << hw_joint_command_;
227+ RCLCPP_INFO (
228+ rclcpp::get_logger (" RRBotActuatorWithoutFeedback" ), " Sending data command: %s" ,
229+ data.str ().c_str ());
230+ send (sock_, data.str ().c_str (), strlen (data.str ().c_str ()), 0 );
231+
232+ RCLCPP_INFO (rclcpp::get_logger (" RRBotActuatorWithoutFeedback" ), " Joints successfully written!" );
233+ // END: This part here is for exemplary purposes - Please do not copy to your production code
233234
234- RCLCPP_INFO (rclcpp::get_logger (" RRBotActuatorWithoutFeedback" ), " Joints successfully written!" );
235- // END: This part here is for exemplary purposes - Please do not copy to your production code
236- }
237235 return hardware_interface::return_type::OK;
238236}
239237
0 commit comments