Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft: Fix deprecation warnings in controllers from recent ros2_control variant changes. #1443

Draft
wants to merge 2 commits into
base: master
Choose a base branch
from
Draft

Draft: Fix deprecation warnings in controllers from recent ros2_control variant changes. #1443

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5edb5f4
Fix warnings in diff_drive_controller
kumar-sanjeeev Dec 22, 2024
1044e67
Update set_value() usuage to fix warning in velocity_controllers
kumar-sanjeeev Dec 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 39 additions & 6 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,9 +159,28 @@ controller_interface::return_type DiffDriveController::update(
double right_feedback_mean = 0.0;
for (size_t index = 0; index < static_cast<size_t>(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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

shall we simplify this and use just a single bool variable if we don't use the information of left/right failure?
Please also add a comment here, that we only need the lambda functions here to be able to mark the double left/right_feedback variable as const.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

  bool success = true;
    // lambda function only to mark `double left_feedback` as `const`.
    const double left_feedback = [&]()
    {
      double temp{};
      success = registered_left_wheel_handles_[index].feedback.get().get_value(temp);
      return temp;
    }();

    // lambda function only to mark `double right_feedback` as `const`.
    const double right_feedback = [&]()
    {
      double temp{};
      success = registered_right_wheel_handles_[index].feedback.get().get_value(temp);
      return temp;
    }();

    if (!success)
    {
      RCLCPP_WARN(
        get_node()->get_logger(),
        "Error while fetching information from the state interfaces for %s wheel at index [%zu]",
        (index % 2 == 0) ? "left" : "right", index);
      return controller_interface::return_type::OK;
    }

@christophfroehlich Is this make sense ?

Copy link
Contributor Author

@kumar-sanjeeev kumar-sanjeeev Dec 23, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

while working on another controller, I noticed that I often need to implement a similar setup for get_value()across multiple places, wouldn't it be a good idea to create a separate reusable function for this? We could then import it wherever needed, which might also be helpful in test files.

for example ( in this case I created single lambda)

    bool success = true;

    // lambda function only to mark return as const
    auto fetch_feedback = [&](const auto & wheel_handles, size_t index) -> double
    {
      double temp{};
      success = wheel_handles[index].feedback.get().get_value(temp);
      return temp;
    };

    for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
    {
      const double left_feedback = fetch_feedback(registered_left_wheel_handles_, index);
      const double right_feedback = fetch_feedback(registered_right_wheel_handles_, index);

      if (!success)
      {
        RCLCPP_WARN(
          get_node()->get_logger(),
          "Error while fetching information from the state interfaces for %s wheel at index [%zu]",
          (index % 2 == 0) ? "left" : "right", index);
        return controller_interface::return_type::OK;
      }

what do you think?

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))
{
Expand Down Expand Up @@ -267,8 +286,17 @@ controller_interface::return_type DiffDriveController::update(
// Set wheels velocities:
for (size_t index = 0; index < static_cast<size_t>(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;
Expand Down Expand Up @@ -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");
}
}
};

Expand Down
6 changes: 5 additions & 1 deletion velocity_controllers/src/joint_group_velocity_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading