diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp index 7a2f99609..837d3aa2e 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp @@ -20,7 +20,6 @@ #define ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_ #include -#include #include #include #include diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp index 8816f7a9b..5644d714d 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -20,7 +20,6 @@ #define ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_SENSOR_FOR_POSITION_FEEDBACK_HPP_ #include -#include #include #include #include diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 6feb8067c..80a24d4b0 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -19,8 +19,10 @@ #include "ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp" #include +#include #include #include +#include #include #include #include @@ -94,7 +96,8 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connection over socket failed."); + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connection over socket failed: %s", + strerror(errno)); // Print the error message return hardware_interface::CallbackReturn::ERROR; } RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connected to socket"); diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp index 395c922c6..96e7ee866 100644 --- a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -18,10 +18,11 @@ #include "ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp" -#include +#include #include #include #include +#include #include #include #include @@ -101,7 +102,9 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket address."); if (bind(obj_socket_, reinterpret_cast(&address_), sizeof(address_)) < 0) { - RCLCPP_FATAL(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket failed."); + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket failed: %s", + strerror(errno)); // Print the error message return hardware_interface::CallbackReturn::ERROR; }