From 5edb5f406b9721d17c5bd8c13e22951fc6fbd3af Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 22 Dec 2024 23:01:39 +0100 Subject: [PATCH 1/2] Fix warnings in diff_drive_controller --- .../src/diff_drive_controller.cpp | 45 ++++++++++++++++--- 1 file changed, 39 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 6957864321..7b04b70a7e 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -159,9 +159,28 @@ controller_interface::return_type DiffDriveController::update( double right_feedback_mean = 0.0; for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { - const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); - const double right_feedback = - registered_right_wheel_handles_[index].feedback.get().get_value(); + bool left_success = false; + const double left_feedback = [&]() + { + double temp{}; + left_success = registered_left_wheel_handles_[index].feedback.get().get_value(temp); + return temp; + }(); + + bool right_success = false; + const double right_feedback = [&]() + { + double temp{}; + right_success = registered_right_wheel_handles_[index].feedback.get().get_value(temp); + return temp; + }(); + + if (!left_success || !right_success) + { + RCLCPP_WARN( + get_node()->get_logger(), "Error while fetching information from the state interfaces"); + return controller_interface::return_type::OK; + } if (std::isnan(left_feedback) || std::isnan(right_feedback)) { @@ -267,8 +286,17 @@ controller_interface::return_type DiffDriveController::update( // Set wheels velocities: for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { - registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); - registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); + if ( + !registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left) || + !registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Error while setting the velocity_left : %f " + "and velocity_right : %f to the command handles", + velocity_left, velocity_right); + return controller_interface::return_type::OK; + } } return controller_interface::return_type::OK; @@ -590,7 +618,12 @@ void DiffDriveController::halt() { for (const auto & wheel_handle : wheel_handles) { - wheel_handle.velocity.get().set_value(0.0); + if (!wheel_handle.velocity.get().set_value(0.0)) + { + RCLCPP_WARN( + rclcpp::get_logger("DiffDriveController"), + "Error while setting the wheel velocity to 0.0"); + } } }; From 1044e677eb70ac63c2768c2aae5db3a451e91e35 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 23 Dec 2024 13:36:09 +0100 Subject: [PATCH 2/2] Update set_value() usuage to fix warning in velocity_controllers --- .../src/joint_group_velocity_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 97f6713d72..40c3870ba2 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -59,7 +59,11 @@ controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate // stop all joints for (auto & command_interface : command_interfaces_) { - command_interface.set_value(0.0); + if (!command_interface.set_value(0.0)) + { + RCLCPP_WARN(get_node()->get_logger(), "Error while setting command_interface value to 0.0"); + return CallbackReturn::FAILURE; + } } return ret;