diff --git a/robotiq_controllers/CMakeLists.txt b/robotiq_controllers/CMakeLists.txt index c4334bc5..afe968f5 100644 --- a/robotiq_controllers/CMakeLists.txt +++ b/robotiq_controllers/CMakeLists.txt @@ -25,8 +25,9 @@ target_include_directories(${PROJECT_NAME} PRIVATE include ) -ament_target_dependencies(${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_link_libraries(${PROJECT_NAME} + controller_interface::controller_interface + ${std_srvs_TARGETS} ) pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml) diff --git a/robotiq_controllers/src/robotiq_activation_controller.cpp b/robotiq_controllers/src/robotiq_activation_controller.cpp index aa63d214..cd28efcf 100644 --- a/robotiq_controllers/src/robotiq_activation_controller.cpp +++ b/robotiq_controllers/src/robotiq_activation_controller.cpp @@ -105,14 +105,21 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Roboti bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/, std_srvs::srv::Trigger::Response::SharedPtr resp) { - command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING); - command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0); + resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING); + resp->success &= command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0); - while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING) + while (true) { + const auto maybe_value = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_optional(); + if (maybe_value && maybe_value.value() != ASYNC_WAITING) + { + break; + } std::this_thread::sleep_for(std::chrono::milliseconds(50)); } - resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value(); + // NOTE: This was previously using get_value() and implicitly casting to bool, so keeping the old behavior. + // However, note that the value of this result is actually a double, so this should be revised in the future. + resp->success &= static_cast(command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_optional().value_or(false)); return resp->success; } diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index df9f8a6b..72f4515d 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -61,9 +61,13 @@ target_include_directories( $ $ ) -ament_target_dependencies( +target_link_libraries( robotiq_driver - ${THIS_PACKAGE_INCLUDE_DEPENDS} + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + serial::serial ) ############################################################################### diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 517249ab..2890e62c 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -72,12 +72,12 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa /** * Initialization of the hardware interface from data parsed from the * robot's URDF. - * @param hardware_info Structure with data from URDF. + * @param params Structure with parameters for initializing this hardware component. * @returns CallbackReturn::SUCCESS if required data are provided and can be * parsed or CallbackReturn::ERROR if any error happens or data are missing. */ ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; + CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; /** * Connect to the hardware. diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index b837af51..e530d737 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -72,22 +72,23 @@ RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface(std::unique_ptr { } -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) +hardware_interface::CallbackReturn +RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareComponentInterfaceParams& params) { RCLCPP_DEBUG(kLogger, "on_init"); - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } // Read parameters. - gripper_closed_pos_ = stod(info_.hardware_parameters["gripper_closed_position"]); + gripper_closed_pos_ = stod(info_.hardware_parameters.at("gripper_closed_position")); gripper_max_speed_ = info_.hardware_parameters.count("gripper_max_speed") ? - stod(info_.hardware_parameters["gripper_max_speed"]) : + stod(info_.hardware_parameters.at("gripper_max_speed")) : kGripperMaxSpeed; gripper_max_force_ = info_.hardware_parameters.count("gripper_max_force") ? - stod(info_.hardware_parameters["gripper_max_force"]) : + stod(info_.hardware_parameters.at("gripper_max_force")) : kGripperMaxforce; gripper_position_ = std::numeric_limits::quiet_NaN(); gripper_velocity_ = std::numeric_limits::quiet_NaN(); @@ -95,7 +96,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons reactivate_gripper_cmd_ = NO_NEW_CMD_; reactivate_gripper_async_cmd_.store(false); - const hardware_interface::ComponentInfo& joint = info_.joints[0]; + const hardware_interface::ComponentInfo& joint = info_.joints.at(0); // There is one command interface: position. if (joint.command_interfaces.size() != 1) @@ -105,10 +106,10 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons return CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + if (joint.command_interfaces.at(0).name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL(kLogger, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + joint.command_interfaces.at(0).name.c_str(), hardware_interface::HW_IF_POSITION); return CallbackReturn::ERROR; } @@ -126,7 +127,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) { RCLCPP_FATAL(kLogger, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(), - joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION, + joint.state_interfaces.at(i).name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); return CallbackReturn::ERROR; } diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt index 5db0b3c8..38af19b3 100644 --- a/robotiq_driver/tests/CMakeLists.txt +++ b/robotiq_driver/tests/CMakeLists.txt @@ -19,8 +19,11 @@ ament_add_gmock(test_robotiq_gripper_hardware_interface target_include_directories(test_robotiq_gripper_hardware_interface PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) -target_link_libraries(test_robotiq_gripper_hardware_interface robotiq_driver) -ament_target_dependencies(test_robotiq_gripper_hardware_interface) +target_link_libraries( + test_robotiq_gripper_hardware_interface + robotiq_driver + hardware_interface::hardware_interface +) ############################################################################### # test_default_serial_factory diff --git a/robotiq_hardware_tests/CMakeLists.txt b/robotiq_hardware_tests/CMakeLists.txt index 668cf319..2ed81ae3 100644 --- a/robotiq_hardware_tests/CMakeLists.txt +++ b/robotiq_hardware_tests/CMakeLists.txt @@ -31,6 +31,6 @@ add_executable(full_test src/command_line_utility.hpp src/command_line_utility.cpp ) -ament_target_dependencies(full_test robotiq_driver) +target_link_libraries(full_test robotiq_driver::robotiq_driver) ament_package()