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

Fix handles for following controllers of diff_drive_controller #1513

Closed
wants to merge 1 commit into from
Closed

Fix handles for following controllers of diff_drive_controller #1513

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
33 changes: 21 additions & 12 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 @@ -673,28 +673,37 @@ controller_interface::CallbackReturn DiffDriveController::configure_side(
const auto state_handle = std::find_if(
state_interfaces_.cbegin(), state_interfaces_.cend(),
[&wheel_name, &interface_name](const auto & interface)
{
return interface.get_prefix_name() == wheel_name &&
interface.get_interface_name() == interface_name;
});
{ return interface.get_name() == wheel_name + "/" + interface_name; });

if (state_handle == state_interfaces_.cend())
{
RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str());
auto full_name = wheel_name + "/" + interface_name;
RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", full_name.c_str());
RCLCPP_ERROR(logger, "Available state interfaces:");
for (const auto & interface : state_interfaces_)
{
RCLCPP_ERROR(
logger, " - %s: %s", interface.get_prefix_name().c_str(),
interface.get_interface_name().c_str());
}
Copy link
Contributor

@Juliaj Juliaj Feb 1, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

I tested the fix above and it works with example_16. However, I wonder these debug message could lead to confusion if the developer is unaware about the chain created under the hood. For example_16, prefix: interface pair is pid_controller_left_wheel_joint : left_wheel_joint/velocity.

A naive alternative to the current fix could be:

  • preserve the prefix and interface as in the current definition
  • introduce a mode (e.g. chained_right) with additional params and logic to handle the use case.

Copy link
Contributor

Choose a reason for hiding this comment

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

Thought about this a bit more. Please ignore my comments on alternative fix above because I believe it is supported by default. This is illustrated by demo example_12 where the forward_position_controller controls "joints" exposed by the second level controller. These (virtual ?) joints are configured in a hierarchical pattern.

# Third-level controllers
forward_position_controller:
  ros__parameters:
    type: forward_command_controller/ForwardCommandController
    joints:
      - position_controller/joint1_position_controller/joint1
      - position_controller/joint2_position_controller/joint2
    interface_name: position

return controller_interface::CallbackReturn::ERROR;
}

const auto command_handle = std::find_if(
command_interfaces_.begin(), command_interfaces_.end(),
[&wheel_name](const auto & interface)
{
return interface.get_prefix_name() == wheel_name &&
interface.get_interface_name() == HW_IF_VELOCITY;
});
command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name](const auto & interface)
{ return interface.get_name() == wheel_name + "/" + HW_IF_VELOCITY; });

if (command_handle == command_interfaces_.end())
{
RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str());
auto full_name = wheel_name + "/" + HW_IF_VELOCITY;
RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", full_name.c_str());
RCLCPP_ERROR(logger, "Available command interfaces:");
for (const auto & interface : command_interfaces_)
{
RCLCPP_ERROR(
logger, " - %s: %s", interface.get_prefix_name().c_str(),
interface.get_interface_name().c_str());
}
return controller_interface::CallbackReturn::ERROR;
}

Expand Down