From 0db2dec9e3131354f085920ef624d418b5c0ec7d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 31 Jan 2025 18:58:21 +0000 Subject: [PATCH] Use get_name() for interface checks --- .../src/diff_drive_controller.cpp | 33 ++++++++++++------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 7356d80219..3f10e082bd 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -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()); + } 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; }