From c7a0dfc22b43e92c119bdef12be8e661c889943e Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 1 Feb 2024 15:08:46 -0700 Subject: [PATCH 01/30] add gripper controller with optional velocity/effort Signed-off-by: Paul Gesel --- antipodal_gripper_controller/CMakeLists.txt | 88 ++++ antipodal_gripper_controller/doc/userdoc.rst | 14 + .../antipodal_gripper_action_controller.hpp | 175 ++++++++ ...tipodal_gripper_action_controller_impl.hpp | 384 ++++++++++++++++++ .../visibility_control.hpp | 56 +++ antipodal_gripper_controller/package.xml | 35 ++ .../ros_control_plugins.xml | 10 + .../antipodal_gripper_action_controller.cpp | 23 ++ ..._gripper_action_controller_parameters.yaml | 53 +++ .../test_antipodal_gripper_controller.cpp | 180 ++++++++ .../test_antipodal_gripper_controller.hpp | 64 +++ ...ad_antipodal_gripper_action_controller.cpp | 45 ++ 12 files changed, 1127 insertions(+) create mode 100644 antipodal_gripper_controller/CMakeLists.txt create mode 100644 antipodal_gripper_controller/doc/userdoc.rst create mode 100644 antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp create mode 100644 antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp create mode 100644 antipodal_gripper_controller/include/antipodal_gripper_controller/visibility_control.hpp create mode 100644 antipodal_gripper_controller/package.xml create mode 100644 antipodal_gripper_controller/ros_control_plugins.xml create mode 100644 antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp create mode 100644 antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml create mode 100644 antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp create mode 100644 antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp create mode 100644 antipodal_gripper_controller/test/test_load_antipodal_gripper_action_controller.cpp diff --git a/antipodal_gripper_controller/CMakeLists.txt b/antipodal_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..a3c20792c3 --- /dev/null +++ b/antipodal_gripper_controller/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.16) +project(antipodal_gripper_controller LANGUAGES CXX) + +if(APPLE OR WIN32) + message(WARNING "antipodal gripper controllers are not available on OSX or Windows") + return() +endif() + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(antipodal_gripper_action_controller_parameters + src/antipodal_gripper_action_controller_parameters.yaml +) + +add_library(antipodal_gripper_action_controller SHARED + src/antipodal_gripper_action_controller.cpp +) +target_compile_features(antipodal_gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(antipodal_gripper_action_controller PUBLIC + $ + $ +) +target_link_libraries(antipodal_gripper_action_controller PUBLIC + antipodal_gripper_action_controller_parameters +) +ament_target_dependencies(antipodal_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_antipodal_gripper_action_controllers + test/test_load_antipodal_gripper_action_controller.cpp + ) + ament_target_dependencies(test_load_antipodal_gripper_action_controllers + controller_manager + ros2_control_test_assets + ) + + ament_add_gmock(test_antipodal_gripper_controller + test/test_antipodal_gripper_controller.cpp + ) + target_link_libraries(test_antipodal_gripper_controller + antipodal_gripper_action_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/antipodal_gripper_action_controller +) +install( + TARGETS + antipodal_gripper_action_controller + antipodal_gripper_action_controller_parameters + EXPORT export_antipodal_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_antipodal_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/antipodal_gripper_controller/doc/userdoc.rst b/antipodal_gripper_controller/doc/userdoc.rst new file mode 100644 index 0000000000..3410f56296 --- /dev/null +++ b/antipodal_gripper_controller/doc/userdoc.rst @@ -0,0 +1,14 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/antipodal_gripper_controller/doc/userdoc.rst + +.. _antipodal_gripper_controller_userdoc: + +Gripper Action Controller +-------------------------------- + +Controller for executing a gripper command action for simple single-dof antipodal grippers. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp new file mode 100644 index 0000000000..9581dce2dd --- /dev/null +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp @@ -0,0 +1,175 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian + +#ifndef ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ +#define ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ + +// C++ standard +#include +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" + +// ROS messages +#include "control_msgs/action/gripper_command.hpp" + +// rclcpp_action +#include "rclcpp_action/create_server.hpp" + +// ros_controls +#include "controller_interface/controller_interface.hpp" +#include "antipodal_gripper_controller/visibility_control.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_server_goal_handle.h" + +// Project +#include "antipodal_gripper_action_controller_parameters.hpp" + +namespace antipodal_gripper_action_controller +{ +/** + * \brief Controller for executing a gripper command action for simple + * single-dof grippers. + * + * \tparam HardwareInterface Controller hardware interface. Currently \p + * hardware_interface::HW_IF_POSITION and \p + * hardware_interface::HW_IF_EFFORT are supported out-of-the-box. + */ + +class GripperActionController : public controller_interface::ControllerInterface +{ +public: + /** + * \brief Store position and max effort in struct to allow easier realtime + * buffer usage + */ + struct Commands + { + double position_; // Last commanded position + double target_velocity_; // Desired gripper velocity + double max_effort_; // Max allowed effort + }; + + GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + + /** + * @brief command_interface_configuration This controller requires the + * position command interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief command_interface_configuration This controller requires the + * position and velocity state interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + realtime_tools::RealtimeBuffer command_; + // pre-allocated memory that is re-used to set the realtime buffer + Commands command_struct_, command_struct_rt_; + +protected: + using GripperCommandAction = control_msgs::action::GripperCommand; + using ActionServer = rclcpp_action::Server; + using ActionServerPtr = ActionServer::SharedPtr; + using GoalHandle = rclcpp_action::ServerGoalHandle; + using RealtimeGoalHandle = + realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + bool update_hold_position_; + + bool verbose_ = false; ///< Hard coded verbose flag to help in debugging + std::string name_; ///< Controller name. + std::optional> + joint_command_interface_; + std::optional> + effort_interface_; + std::optional> + speed_interface_; + std::optional> + joint_position_state_interface_; + std::optional> + joint_velocity_state_interface_; + + std::shared_ptr param_listener_; + Params params_; + + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. + control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; + + rclcpp::Duration action_monitor_period_; + + // ROS API + ActionServerPtr action_server_; + + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + + void accepted_callback(std::shared_ptr goal_handle); + + void preempt_active_goal(); + + void set_hold_position(); + + rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time + double computed_command_; ///< Computed command + + /** + * \brief Check for success and publish appropriate result and feedback. + **/ + void check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity); +}; + +} // namespace antipodal_gripper_action_controller + +#include "antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp" + +#endif // ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp new file mode 100644 index 0000000000..57d0ad1c02 --- /dev/null +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -0,0 +1,384 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser + +#ifndef ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ + +#include "antipodal_gripper_controller/antipodal_gripper_action_controller.hpp" + +#include +#include +#include + +namespace antipodal_gripper_action_controller +{ + +void GripperActionController::preempt_active_goal() +{ + // Cancels the currently active goal + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + // Marks the current goal as canceled + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } +} + +controller_interface::CallbackReturn GripperActionController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GripperActionController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + command_struct_rt_ = *(command_.readFromRT()); + + const double current_position = joint_position_state_interface_->get().get_value(); + const double current_velocity = joint_velocity_state_interface_->get().get_value(); + + const double error_position = command_struct_rt_.position_ - current_position; + const double error_velocity = command_struct_rt_.target_velocity_ - current_velocity; + + check_for_success(get_node()->now(), error_position, current_position, current_velocity); + + joint_command_interface_->get().set_value(command_struct_rt_.position_); + if (speed_interface_.has_value()) + { + speed_interface_->get().set_value(command_struct_rt_.target_velocity_); + } + if (effort_interface_.has_value()) + { + effort_interface_->get().set_value(command_struct_rt_.max_effort_); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse GripperActionController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void GripperActionController::accepted_callback( + std::shared_ptr goal_handle) // Try to update goal +{ + auto rt_goal = std::make_shared(goal_handle); + + // Accept new goal + preempt_active_goal(); + + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_ = goal_handle->get_goal()->command.position; + if (params_.use_velocity_interface) + { + command_struct_.target_velocity_ = goal_handle->get_goal()->command.target_velocity; + } + else + { + command_struct_.target_velocity_ = 0.0; + } + if (params_.use_effort_interface) + { + command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + } + else + { + command_struct_.max_effort_ = params_.max_effort; + } + command_.writeFromNonRT(command_struct_); + + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); +} + +rclcpp_action::CancelResponse GripperActionController::cancel_callback( + const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) + { + // Enter hold current position mode + set_hold_position(); + + RCLCPP_INFO( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + // Reset current goal + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperActionController::set_hold_position() +{ + command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.target_velocity_ = 0.0; + command_struct_.max_effort_ = params_.max_effort; + command_.writeFromNonRT(command_struct_); +} + +void GripperActionController::check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity) +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) + { + return; + } + + if (fabs(error_position) < params_.goal_tolerance) + { + pre_alloc_result_->effort = computed_command_; + pre_alloc_result_->position = current_position; + pre_alloc_result_->reached_goal = true; + pre_alloc_result_->stalled = false; + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + else + { + if (fabs(current_velocity) > params_.stall_velocity_threshold) + { + last_movement_time_ = time; + } + else if ((time - last_movement_time_).seconds() > params_.stall_timeout) + { + pre_alloc_result_->effort = computed_command_; + pre_alloc_result_->position = current_position; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = true; + + if (params_.allow_stalling) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); + active_goal->setSucceeded(pre_alloc_result_); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); + active_goal->setAborted(pre_alloc_result_); + } + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + } +} + +controller_interface::CallbackReturn GripperActionController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + // Action status checking update rate + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); + RCLCPP_INFO_STREAM( + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + + // Controlled joint + if (params_.joint.empty()) + { + RCLCPP_ERROR(logger, "Joint name cannot be empty"); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_activate( + const rclcpp_lifecycle::State &) +{ + auto command_interface_it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [](const hardware_interface::LoanedCommandInterface & command_interface) + { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (command_interface_it == command_interfaces_.end()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Expected 1 " << hardware_interface::HW_IF_POSITION << " command interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (command_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto position_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (position_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (position_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Position state interface is different than joint name `" + << position_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto velocity_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); + if (velocity_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (velocity_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Velocity command interface is different than joint name `" + << velocity_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + + joint_command_interface_ = *command_interface_it; + joint_position_state_interface_ = *position_state_interface_it; + joint_velocity_state_interface_ = *velocity_state_interface_it; + + for (auto & interface : command_interfaces_) + { + if (interface.get_interface_name() == "gripper_effort") + { + effort_interface_ = interface; + } + else if (interface.get_interface_name() == "gripper_speed") + { + speed_interface_ = interface; + } + } + + // Hardware interface adapter + // hw_iface_adapter_.init(joint_command_interface_, speed_interface_, effort_interface_, + // get_node()); + + // Command - non RT version + command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.target_velocity_ = 0.0; + command_.initRT(command_struct_); + + // Result + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->position = command_struct_.position_; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + // Action interface + action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_cmd", + std::bind( + &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), + std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + joint_command_interface_ = std::nullopt; + joint_position_state_interface_ = std::nullopt; + joint_velocity_state_interface_ = std::nullopt; + release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GripperActionController::command_interface_configuration() const +{ + std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; + if (params_.use_effort_interface) + { + names.push_back({params_.joint + "/gripper_effort"}); + } + if (params_.use_velocity_interface) + { + names.push_back({params_.joint + "/gripper_speed"}); + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; +} + +controller_interface::InterfaceConfiguration +GripperActionController::state_interface_configuration() const +{ + return { + controller_interface::interface_configuration_type::INDIVIDUAL, + {params_.joint + "/" + hardware_interface::HW_IF_POSITION, + params_.joint + "/" + hardware_interface::HW_IF_VELOCITY}}; +} + +GripperActionController::GripperActionController() +: controller_interface::ControllerInterface(), + action_monitor_period_(rclcpp::Duration::from_seconds(0)) +{ +} + +} // namespace antipodal_gripper_action_controller + +#endif // ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/visibility_control.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/visibility_control.hpp new file mode 100644 index 0000000000..7e305c720d --- /dev/null +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ANTIPODAL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#define ANTIPODAL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // ANTIPODAL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/antipodal_gripper_controller/package.xml b/antipodal_gripper_controller/package.xml new file mode 100644 index 0000000000..8fe2ecdfa4 --- /dev/null +++ b/antipodal_gripper_controller/package.xml @@ -0,0 +1,35 @@ + + + + antipodal_gripper_controller + 2.32.0 + The antipodal_gripper_controller package + Bence Magyar + + Apache License 2.0 + + Sachin Chitta + + ament_cmake + + backward_ros + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/antipodal_gripper_controller/ros_control_plugins.xml b/antipodal_gripper_controller/ros_control_plugins.xml new file mode 100644 index 0000000000..a276f3dd1b --- /dev/null +++ b/antipodal_gripper_controller/ros_control_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp b/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp new file mode 100644 index 0000000000..949388cd66 --- /dev/null +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp @@ -0,0 +1,23 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta + +// Project +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(antipodal_gripper_action_controller::GripperActionController, controller_interface::ControllerInterface) \ No newline at end of file diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..fca6890787 --- /dev/null +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -0,0 +1,53 @@ +antipodal_gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + validation: { + gt_eq: [ 0.1 ] + }, + } + joint: { + type: string, + default_value: "", + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + gt_eq: [ 0.0 ] + }, + } + max_effort: { + type: double, + default_value: 0.0, + description: "Max allowable effort", + validation: { + gt_eq: [ 0.0 ] + }, + } + allow_stalling: { + type: bool, + description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", + default_value: false, + } + stall_velocity_threshold: { + type: double, + description: "stall velocity threshold", + default_value: 0.001, + } + stall_timeout: { + type: double, + description: "stall timeout", + default_value: 1.0, + } + use_effort_interface: { + type: bool, + description: "Controller will claim the {joint}/gripper_effort interface if true.", + default_value: false, + } + use_velocity_interface: { + type: bool, + description: "Controller will claim the {joint}/gripper_speed interface if true.", + default_value: false, + } diff --git a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp new file mode 100644 index 0000000000..63a48fa41e --- /dev/null +++ b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp @@ -0,0 +1,180 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "test_antipodal_gripper_controller.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using GripperCommandAction = control_msgs::action::GripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; + + +void GripperControllerTest::SetUpTestCase() +{ + rclcpp::init(0, nullptr); +} + + +void GripperControllerTest::TearDownTestCase() +{ + rclcpp::shutdown(); +} + + +void GripperControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + + +void GripperControllerTest::TearDown() +{ + controller_.reset(nullptr); +} + + +void GripperControllerTest::SetUpController() +{ + const auto result = controller_->init("gripper_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + this->SetUpController(); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, JointParameterIsEmpty) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", ""}); + + // configure failed, 'joints' is empty + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ConfigureParamsSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint_1"}); + + // configure successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + + // activate failed, 'joint4' is not a valid joint name for the hardware + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + // activate successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // re-assign interfaces + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} diff --git a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp new file mode 100644 index 0000000000..53daf48acc --- /dev/null +++ b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp @@ -0,0 +1,64 @@ +// Copyright 2022 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ANTIPODAL_GRIPPER_CONTROLLER_HPP_ +#define TEST_ANTIPODAL_GRIPPER_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "antipodal_gripper_controller/antipodal_gripper_action_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +namespace +{ +// subclassing and friending so we can access member variables +class FriendGripperController +: public antipodal_gripper_action_controller::GripperActionController +{ + FRIEND_TEST(GripperControllerTest, CommandSuccessTest); +}; + +class GripperControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::string joint_name_ = "joint1"; + std::vector joint_states_ = {1.1, 2.1}; + std::vector joint_commands_ = {3.1}; + + hardware_interface::StateInterface joint_1_pos_state_{joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; + hardware_interface::StateInterface joint_1_vel_state_{joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; + hardware_interface::CommandInterface joint_1_cmd_{joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; +}; + +} // anonymous namespace + +#endif // TEST_ANTIPODAL_GRIPPER_CONTROLLER_HPP_ diff --git a/antipodal_gripper_controller/test/test_load_antipodal_gripper_action_controller.cpp b/antipodal_gripper_controller/test/test_load_antipodal_gripper_action_controller.cpp new file mode 100644 index 0000000000..130b12e0bb --- /dev/null +++ b/antipodal_gripper_controller/test/test_load_antipodal_gripper_action_controller.cpp @@ -0,0 +1,45 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGripperActionControllers, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_gripper_action_position_controller", "position_controllers/GripperActionController"), + nullptr); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + nullptr); + + rclcpp::shutdown(); +} From 9e8b95a80928f2fb91c190fd3514919d2c4ae008 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 1 Feb 2024 16:09:28 -0700 Subject: [PATCH 02/30] add deprecation warning + fix test Signed-off-by: Paul Gesel --- .../antipodal_gripper_action_controller.hpp | 10 ++--- ...tipodal_gripper_action_controller_impl.hpp | 42 ++++++++++++------- .../ros_control_plugins.xml | 6 +-- .../antipodal_gripper_action_controller.cpp | 4 +- .../test_antipodal_gripper_controller.cpp | 26 ++++-------- .../test_antipodal_gripper_controller.hpp | 12 +++--- .../gripper_action_controller_impl.hpp | 5 +++ 7 files changed, 57 insertions(+), 48 deletions(-) diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp index 9581dce2dd..544a0abb0e 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp @@ -27,14 +27,14 @@ #include "rclcpp/rclcpp.hpp" // ROS messages -#include "control_msgs/action/gripper_command.hpp" +#include "control_msgs/action/antipodal_gripper_command.hpp" // rclcpp_action #include "rclcpp_action/create_server.hpp" // ros_controls -#include "controller_interface/controller_interface.hpp" #include "antipodal_gripper_controller/visibility_control.hpp" +#include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "realtime_tools/realtime_buffer.h" @@ -108,12 +108,12 @@ class GripperActionController : public controller_interface::ControllerInterface Commands command_struct_, command_struct_rt_; protected: - using GripperCommandAction = control_msgs::action::GripperCommand; + using GripperCommandAction = control_msgs::action::AntipodalGripperCommand; using ActionServer = rclcpp_action::Server; using ActionServerPtr = ActionServer::SharedPtr; using GoalHandle = rclcpp_action::ServerGoalHandle; using RealtimeGoalHandle = - realtime_tools::RealtimeServerGoalHandle; + realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; @@ -137,7 +137,7 @@ class GripperActionController : public controller_interface::ControllerInterface RealtimeGoalHandleBuffer rt_active_goal_; ///< Container for the currently active action goal, if any. - control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; + control_msgs::action::AntipodalGripperCommand::Result::SharedPtr pre_alloc_result_; rclcpp::Duration action_monitor_period_; diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index 57d0ad1c02..66af53bab8 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -61,9 +61,7 @@ controller_interface::return_type GripperActionController::update( const double current_position = joint_position_state_interface_->get().get_value(); const double current_velocity = joint_velocity_state_interface_->get().get_value(); - const double error_position = command_struct_rt_.position_ - current_position; - const double error_velocity = command_struct_rt_.target_velocity_ - current_velocity; check_for_success(get_node()->now(), error_position, current_position, current_velocity); @@ -81,8 +79,20 @@ controller_interface::return_type GripperActionController::update( } rclcpp_action::GoalResponse GripperActionController::goal_callback( - const rclcpp_action::GoalUUID &, std::shared_ptr) + const rclcpp_action::GoalUUID &, std::shared_ptr goal_handle) { + if (goal_handle->command.position.size() != 1) + { + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + RCLCPP_ERROR( + get_node()->get_logger(), + "Received action goal with wrong number of position values, expects 1, got %zu", + goal_handle->command.position.size()); + return rclcpp_action::GoalResponse::REJECT; + } + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -97,18 +107,18 @@ void GripperActionController::accepted_callback( // This is the non-realtime command_struct // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position; - if (params_.use_velocity_interface) + command_struct_.position_ = goal_handle->get_goal()->command.position[0]; + if (params_.use_velocity_interface && !goal_handle->get_goal()->command.velocity.empty()) { - command_struct_.target_velocity_ = goal_handle->get_goal()->command.target_velocity; + command_struct_.target_velocity_ = goal_handle->get_goal()->command.velocity[0]; } else { command_struct_.target_velocity_ = 0.0; } - if (params_.use_effort_interface) + if (params_.use_effort_interface && !goal_handle->get_goal()->command.effort.empty()) { - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; } else { @@ -176,8 +186,8 @@ void GripperActionController::check_for_success( if (fabs(error_position) < params_.goal_tolerance) { - pre_alloc_result_->effort = computed_command_; - pre_alloc_result_->position = current_position; + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; pre_alloc_result_->reached_goal = true; pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); @@ -192,8 +202,8 @@ void GripperActionController::check_for_success( } else if ((time - last_movement_time_).seconds() > params_.stall_timeout) { - pre_alloc_result_->effort = computed_command_; - pre_alloc_result_->position = current_position; + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = true; @@ -322,13 +332,15 @@ controller_interface::CallbackReturn GripperActionController::on_activate( command_.initRT(command_struct_); // Result - pre_alloc_result_ = std::make_shared(); - pre_alloc_result_->position = command_struct_.position_; + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + pre_alloc_result_->state.position[0] = command_struct_.position_; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false; // Action interface - action_server_ = rclcpp_action::create_server( + action_server_ = rclcpp_action::create_server( get_node(), "~/gripper_cmd", std::bind( &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), diff --git a/antipodal_gripper_controller/ros_control_plugins.xml b/antipodal_gripper_controller/ros_control_plugins.xml index a276f3dd1b..9c20e14360 100644 --- a/antipodal_gripper_controller/ros_control_plugins.xml +++ b/antipodal_gripper_controller/ros_control_plugins.xml @@ -1,7 +1,7 @@ - + - diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp b/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp index 949388cd66..f7b6c651a8 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp @@ -20,4 +20,6 @@ #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(antipodal_gripper_action_controller::GripperActionController, controller_interface::ControllerInterface) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS( + antipodal_gripper_action_controller::GripperActionController, + controller_interface::ControllerInterface) diff --git a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp index 63a48fa41e..5633138213 100644 --- a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp +++ b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp @@ -29,23 +29,14 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; -using GripperCommandAction = control_msgs::action::GripperCommand; +using GripperCommandAction = control_msgs::action::AntipodalGripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; using testing::SizeIs; using testing::UnorderedElementsAre; +void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void GripperControllerTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} - - -void GripperControllerTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} - +void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } void GripperControllerTest::SetUp() { @@ -53,12 +44,7 @@ void GripperControllerTest::SetUp() controller_ = std::make_unique(); } - -void GripperControllerTest::TearDown() -{ - controller_.reset(nullptr); -} - +void GripperControllerTest::TearDown() { controller_.reset(nullptr); } void GripperControllerTest::SetUpController() { @@ -109,7 +95,9 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) // check interface configuration auto cmd_if_conf = this->controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); - ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = this->controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); diff --git a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp index 53daf48acc..7b3a7036f7 100644 --- a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp +++ b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp @@ -28,8 +28,7 @@ namespace { // subclassing and friending so we can access member variables -class FriendGripperController -: public antipodal_gripper_action_controller::GripperActionController +class FriendGripperController : public antipodal_gripper_action_controller::GripperActionController { FRIEND_TEST(GripperControllerTest, CommandSuccessTest); }; @@ -54,9 +53,12 @@ class GripperControllerTest : public ::testing::Test std::vector joint_states_ = {1.1, 2.1}; std::vector joint_commands_ = {3.1}; - hardware_interface::StateInterface joint_1_pos_state_{joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; - hardware_interface::StateInterface joint_1_vel_state_{joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; - hardware_interface::CommandInterface joint_1_cmd_{joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; + hardware_interface::StateInterface joint_1_pos_state_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; + hardware_interface::StateInterface joint_1_vel_state_{ + joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; + hardware_interface::CommandInterface joint_1_cmd_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; }; } // anonymous namespace diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 77598591ae..76fb14fcd9 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -40,6 +40,11 @@ void GripperActionController::preempt_active_goal() template controller_interface::CallbackReturn GripperActionController::on_init() { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: the `position_controllers/GripperActionController` and " + "`effort_controllers::GripperActionController` controllers are replaced by " + "'antipodal_gripper_controllers/GripperActionController' controller"); try { param_listener_ = std::make_shared(get_node()); From 9a4f9f520f9a21a59f0b6bf7a44071e3f96559fe Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 5 Feb 2024 09:43:26 -0700 Subject: [PATCH 03/30] update not released repos Signed-off-by: Paul Gesel --- ros2_controllers-not-released.rolling.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 66352f4960..30366b5406 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -3,3 +3,7 @@ repositories: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git version: main + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: pr-add-gripper-velocity-target-main From 9cb63e80105a88a9dcb770f451cb958700cc4665 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 07:22:08 +0100 Subject: [PATCH 04/30] Use correct ref for scheduled workflows (#1013) --- .github/workflows/humble-debian-build.yml | 1 + .github/workflows/humble-rhel-binary-build.yml | 1 + .github/workflows/iron-debian-build.yml | 1 + .github/workflows/iron-rhel-binary-build.yml | 1 + .github/workflows/rolling-debian-build.yml | 2 ++ .github/workflows/rolling-rhel-binary-build.yml | 2 ++ 6 files changed, 8 insertions(+) diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index e8deb2caa5..426b935fa4 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index cd9b85b2e1..933486ba50 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -19,6 +19,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - name: Install dependencies run: | rosdep update diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index 09dbd051b2..c47fbe5cd9 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index 0eb28b9673..c3bc1e6def 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - name: Install dependencies run: | rosdep update diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index b6d0a4193a..9169494b00 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -20,6 +20,8 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index dece43b673..98c02b72a3 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -20,6 +20,8 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - name: Install dependencies run: | rosdep update From 9ae38f192ca259964ef05efe3638e44175697227 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 09:38:34 +0100 Subject: [PATCH 05/30] Add test_depend on `hardware_interface_testing` (#1018) --- ackermann_steering_controller/package.xml | 1 + admittance_controller/package.xml | 1 + bicycle_steering_controller/package.xml | 1 + diff_drive_controller/test/test_load_diff_drive_controller.cpp | 3 +++ effort_controllers/package.xml | 2 ++ .../test/test_load_joint_group_effort_controller.cpp | 2 ++ force_torque_sensor_broadcaster/package.xml | 1 + forward_command_controller/package.xml | 1 + gripper_controllers/package.xml | 1 + .../test/test_load_gripper_action_controllers.cpp | 2 ++ imu_sensor_broadcaster/package.xml | 1 + joint_state_broadcaster/package.xml | 1 + joint_trajectory_controller/package.xml | 1 + pid_controller/package.xml | 1 + position_controllers/package.xml | 2 ++ .../test/test_load_joint_group_position_controller.cpp | 2 ++ range_sensor_broadcaster/package.xml | 2 +- tricycle_controller/package.xml | 1 + tricycle_controller/test/test_load_tricycle_controller.cpp | 2 ++ tricycle_steering_controller/package.xml | 2 +- velocity_controllers/package.xml | 1 + .../test/test_load_joint_group_velocity_controller.cpp | 2 ++ 22 files changed, 31 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 366c5c31cf..fe22ca10b8 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index d379438824..e690330aa0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -34,6 +34,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing kinematics_interface_kdl ros2_control_test_assets diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index cddfcf3975..8bb6ac79fa 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -28,6 +28,7 @@ ament_cmake_gmock controller_manager hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 983ec6d98f..4c9d2f984f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -16,6 +16,9 @@ #include #include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ce2700ef01..279d5fbf43 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 61bb1ddf9a..52f1f9934a 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1ebd0c4a79..0791eb5d16 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -23,6 +23,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index cae9f877b7..8950a9a3e9 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -22,6 +22,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 348cf6dd93..a35fce7894 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 130b12e0bb..0ef5f0bcb2 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadGripperActionControllers, load_controller) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b6fc59d85c..5694e1cee7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -25,6 +25,7 @@ ament_lint_auto ament_lint_common controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 43dafb66b0..6dd8b4b61e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -25,6 +25,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface rclcpp ros2_control_test_assets diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 030d91d3e9..8cd2e5becc 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -28,6 +28,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 4aa553f31a..70c7bfa987 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e6a98ef7d2..e67d3d8a46 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index fe61039fdb..bc27b5e629 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupPositionController, load_controller) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index c74acce857..2d865c1d7f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -22,7 +22,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index d53e8473a1..4a8725810b 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -30,6 +30,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 245523c844..bd54459780 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -21,7 +21,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadTricycleController, load_controller) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 0263f8f9fe..16bfd522f7 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -29,7 +29,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 94d61b8ae1..3e28f7736e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -17,6 +17,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 1872b5f746..e426349f96 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller) From f22922e1feafb2deaba518bf5e2c7885c4ea3eac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 11:04:49 +0100 Subject: [PATCH 06/30] Add test_depend on `hardware_interface_testing` also for diff_drive (#1021) --- diff_drive_controller/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4531a5337d..e87ab85471 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -26,6 +26,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets From 6b83c4b0109180b6e104933da55cf75278ff4bbd Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 6 Feb 2024 17:04:42 +0100 Subject: [PATCH 07/30] Bump codecov/codecov-action from 3.1.5 to 4.0.1 (#1023) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.5 to 4.0.1. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.5...v4.0.1) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index c503e090ab..931a715a17 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 62ff5f34fe..06eadf320d 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 3f6dff5ab8..2fdbf677e7 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests From f06c43dc8fa9ed2c452765fe5ee2a3809cb5f453 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 6 Feb 2024 17:05:48 +0100 Subject: [PATCH 08/30] Bump actions/upload-artifact from 4.3.0 to 4.3.1 (#1024) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 4.3.0 to 4.3.1. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v4.3.0...v4.3.1) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Dr. Denis --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 931a715a17..357ee3dfa8 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 06eadf320d..7914a1acb0 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 2fdbf677e7..b96276ca5a 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 2a3aa21325..3d5bc1cf35 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 38cb47450abd4ff0124f75283fc07a1d1b45c926 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Feb 2024 15:31:57 -0700 Subject: [PATCH 09/30] Update antipodal_gripper_controller/doc/userdoc.rst Co-authored-by: Marq Rasmussen --- antipodal_gripper_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/antipodal_gripper_controller/doc/userdoc.rst b/antipodal_gripper_controller/doc/userdoc.rst index 3410f56296..90511c376d 100644 --- a/antipodal_gripper_controller/doc/userdoc.rst +++ b/antipodal_gripper_controller/doc/userdoc.rst @@ -2,7 +2,7 @@ .. _antipodal_gripper_controller_userdoc: -Gripper Action Controller +Antipodal Gripper Action Controller -------------------------------- Controller for executing a gripper command action for simple single-dof antipodal grippers. From 7eef1910abf853daea83ef895145c99cff211071 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Feb 2024 15:32:03 -0700 Subject: [PATCH 10/30] Update antipodal_gripper_controller/CMakeLists.txt Co-authored-by: Marq Rasmussen --- antipodal_gripper_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/antipodal_gripper_controller/CMakeLists.txt b/antipodal_gripper_controller/CMakeLists.txt index a3c20792c3..af5ca08921 100644 --- a/antipodal_gripper_controller/CMakeLists.txt +++ b/antipodal_gripper_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(antipodal_gripper_controller LANGUAGES CXX) if(APPLE OR WIN32) - message(WARNING "antipodal gripper controllers are not available on OSX or Windows") + message(WARNING "Antipodal gripper controllers are not available on OSX or Windows") return() endif() From 8b6a5bd19ecee762b777880f2a92f562d3c77392 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Feb 2024 15:32:09 -0700 Subject: [PATCH 11/30] Update antipodal_gripper_controller/doc/userdoc.rst Co-authored-by: Marq Rasmussen --- antipodal_gripper_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/antipodal_gripper_controller/doc/userdoc.rst b/antipodal_gripper_controller/doc/userdoc.rst index 90511c376d..a000e2c863 100644 --- a/antipodal_gripper_controller/doc/userdoc.rst +++ b/antipodal_gripper_controller/doc/userdoc.rst @@ -5,7 +5,7 @@ Antipodal Gripper Action Controller -------------------------------- -Controller for executing a gripper command action for simple single-dof antipodal grippers. +Controller for executing a `AntipodalGripperCommand` action for simple antipodal grippers. Parameters ^^^^^^^^^^^ From 1d2f8b0fd51559050d191a2c236edbf3d14f3508 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Feb 2024 15:32:31 -0700 Subject: [PATCH 12/30] Update antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp Co-authored-by: Marq Rasmussen --- .../antipodal_gripper_action_controller.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp index 544a0abb0e..43c38b0c1f 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp @@ -46,8 +46,7 @@ namespace antipodal_gripper_action_controller { /** - * \brief Controller for executing a gripper command action for simple - * single-dof grippers. + * \brief Controller for executing a `AntipodalGripperCommand` action. * * \tparam HardwareInterface Controller hardware interface. Currently \p * hardware_interface::HW_IF_POSITION and \p From 5a11b3eb476502498ad5e438ba79a69aa516d3d9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Feb 2024 15:33:09 -0700 Subject: [PATCH 13/30] Update antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp Co-authored-by: Marq Rasmussen --- .../antipodal_gripper_action_controller.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp index 43c38b0c1f..c38b9189e1 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp @@ -62,9 +62,9 @@ class GripperActionController : public controller_interface::ControllerInterface */ struct Commands { - double position_; // Last commanded position - double target_velocity_; // Desired gripper velocity - double max_effort_; // Max allowed effort + double position_cmd; // Commanded position + double max_velocity_; // Desired max gripper velocity + double max_effort_; // Desired max allowed effort }; GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); From 6bdacf3382f3311c5962081cbcaf6c5019db320b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Feb 2024 15:47:07 -0700 Subject: [PATCH 14/30] fix use of commmand struct Signed-off-by: Paul Gesel --- antipodal_gripper_controller/doc/userdoc.rst | 2 +- .../antipodal_gripper_action_controller.hpp | 4 ++-- ...tipodal_gripper_action_controller_impl.hpp | 22 +++++++++---------- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/antipodal_gripper_controller/doc/userdoc.rst b/antipodal_gripper_controller/doc/userdoc.rst index a000e2c863..dbbb1d13b7 100644 --- a/antipodal_gripper_controller/doc/userdoc.rst +++ b/antipodal_gripper_controller/doc/userdoc.rst @@ -5,7 +5,7 @@ Antipodal Gripper Action Controller -------------------------------- -Controller for executing a `AntipodalGripperCommand` action for simple antipodal grippers. +Controller for executing a ``AntipodalGripperCommand`` action for simple antipodal grippers. Parameters ^^^^^^^^^^^ diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp index c38b9189e1..45b7697528 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp @@ -62,9 +62,9 @@ class GripperActionController : public controller_interface::ControllerInterface */ struct Commands { - double position_cmd; // Commanded position + double position_cmd_; // Commanded position double max_velocity_; // Desired max gripper velocity - double max_effort_; // Desired max allowed effort + double max_effort_; // Desired max allowed effort }; GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index 66af53bab8..97fe6c5300 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -61,14 +61,14 @@ controller_interface::return_type GripperActionController::update( const double current_position = joint_position_state_interface_->get().get_value(); const double current_velocity = joint_velocity_state_interface_->get().get_value(); - const double error_position = command_struct_rt_.position_ - current_position; + const double error_position = command_struct_rt_.position_cmd_ - current_position; check_for_success(get_node()->now(), error_position, current_position, current_velocity); - joint_command_interface_->get().set_value(command_struct_rt_.position_); + joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); if (speed_interface_.has_value()) { - speed_interface_->get().set_value(command_struct_rt_.target_velocity_); + speed_interface_->get().set_value(command_struct_rt_.max_velocity_); } if (effort_interface_.has_value()) { @@ -107,14 +107,14 @@ void GripperActionController::accepted_callback( // This is the non-realtime command_struct // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position[0]; + command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; if (params_.use_velocity_interface && !goal_handle->get_goal()->command.velocity.empty()) { - command_struct_.target_velocity_ = goal_handle->get_goal()->command.velocity[0]; + command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; } else { - command_struct_.target_velocity_ = 0.0; + command_struct_.max_velocity_ = 0.0; } if (params_.use_effort_interface && !goal_handle->get_goal()->command.effort.empty()) { @@ -168,8 +168,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( void GripperActionController::set_hold_position() { - command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.target_velocity_ = 0.0; + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_velocity_ = 0.0; command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -326,16 +326,16 @@ controller_interface::CallbackReturn GripperActionController::on_activate( // get_node()); // Command - non RT version - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; - command_struct_.target_velocity_ = 0.0; + command_struct_.max_velocity_ = 0.0; command_.initRT(command_struct_); // Result pre_alloc_result_ = std::make_shared(); pre_alloc_result_->state.position.resize(1); pre_alloc_result_->state.effort.resize(1); - pre_alloc_result_->state.position[0] = command_struct_.position_; + pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false; From f9a5a66b4ebdd3f7391d3d51c6582f504989418c Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 13 Feb 2024 09:20:29 -0700 Subject: [PATCH 15/30] update parameters Signed-off-by: Paul Gesel --- .../antipodal_gripper_action_controller_impl.hpp | 6 +++--- ...tipodal_gripper_action_controller_parameters.yaml | 12 ++++++++++-- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index 97fe6c5300..8b5243fd40 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -114,7 +114,7 @@ void GripperActionController::accepted_callback( } else { - command_struct_.max_velocity_ = 0.0; + command_struct_.max_velocity_ = params_.max_velocity; } if (params_.use_effort_interface && !goal_handle->get_goal()->command.effort.empty()) { @@ -169,8 +169,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( void GripperActionController::set_hold_position() { command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); - command_struct_.max_velocity_ = 0.0; command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; command_.writeFromNonRT(command_struct_); } @@ -328,7 +328,7 @@ controller_interface::CallbackReturn GripperActionController::on_activate( // Command - non RT version command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; - command_struct_.max_velocity_ = 0.0; + command_struct_.max_velocity_ = params_.max_velocity; command_.initRT(command_struct_); // Result diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml index fca6890787..5c785fb710 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -20,8 +20,16 @@ antipodal_gripper_action_controller: } max_effort: { type: double, - default_value: 0.0, - description: "Max allowable effort", + default_value: 10.0, + description: "Default effort used for grasping (Newtons)", + validation: { + gt_eq: [ 0.0 ] + }, + } + max_velocity: { + type: double, + default_value: 0.1, + description: "Default target velocity used for grasping (meters/second)", validation: { gt_eq: [ 0.0 ] }, From 61a3fee53fa162a8a0d0d05e05a9f2d7b258fafb Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 13 Feb 2024 10:32:00 -0700 Subject: [PATCH 16/30] update interface names Signed-off-by: Paul Gesel --- .../antipodal_gripper_action_controller_impl.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index 8b5243fd40..3c9d96e90d 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -114,7 +114,7 @@ void GripperActionController::accepted_callback( } else { - command_struct_.max_velocity_ = params_.max_velocity; + command_struct_.max_velocity_ = params_.max_velocity; } if (params_.use_effort_interface && !goal_handle->get_goal()->command.effort.empty()) { @@ -311,11 +311,11 @@ controller_interface::CallbackReturn GripperActionController::on_activate( for (auto & interface : command_interfaces_) { - if (interface.get_interface_name() == "gripper_effort") + if (interface.get_interface_name() == "set_gripper_max_effort") { effort_interface_ = interface; } - else if (interface.get_interface_name() == "gripper_speed") + else if (interface.get_interface_name() == "set_gripper_max_velocity") { speed_interface_ = interface; } @@ -366,11 +366,11 @@ GripperActionController::command_interface_configuration() const std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; if (params_.use_effort_interface) { - names.push_back({params_.joint + "/gripper_effort"}); + names.push_back({params_.joint + "/set_gripper_max_effort"}); } if (params_.use_velocity_interface) { - names.push_back({params_.joint + "/gripper_speed"}); + names.push_back({params_.joint + "/set_gripper_max_velocity"}); } return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; From 114012b5e66fcdb97a65d45a950c3be24ac2ee8f Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 14 Feb 2024 17:21:28 -0700 Subject: [PATCH 17/30] update parameter name Signed-off-by: Paul Gesel --- .../antipodal_gripper_action_controller_impl.hpp | 9 +++++---- .../antipodal_gripper_action_controller_parameters.yaml | 4 ++-- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index 3c9d96e90d..aa05e10486 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -108,7 +108,8 @@ void GripperActionController::accepted_callback( // This is the non-realtime command_struct // We use command_ for sharing command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; - if (params_.use_velocity_interface && !goal_handle->get_goal()->command.velocity.empty()) + if ( + params_.configure_max_velocity_interface && !goal_handle->get_goal()->command.velocity.empty()) { command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; } @@ -116,7 +117,7 @@ void GripperActionController::accepted_callback( { command_struct_.max_velocity_ = params_.max_velocity; } - if (params_.use_effort_interface && !goal_handle->get_goal()->command.effort.empty()) + if (params_.configure_max_effort_interface && !goal_handle->get_goal()->command.effort.empty()) { command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; } @@ -364,11 +365,11 @@ controller_interface::InterfaceConfiguration GripperActionController::command_interface_configuration() const { std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; - if (params_.use_effort_interface) + if (params_.configure_max_effort_interface) { names.push_back({params_.joint + "/set_gripper_max_effort"}); } - if (params_.use_velocity_interface) + if (params_.configure_max_velocity_interface) { names.push_back({params_.joint + "/set_gripper_max_velocity"}); } diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml index 5c785fb710..a08d7596f8 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -49,12 +49,12 @@ antipodal_gripper_action_controller: description: "stall timeout", default_value: 1.0, } - use_effort_interface: { + configure_max_effort_interface: { type: bool, description: "Controller will claim the {joint}/gripper_effort interface if true.", default_value: false, } - use_velocity_interface: { + configure_max_velocity_interface: { type: bool, description: "Controller will claim the {joint}/gripper_speed interface if true.", default_value: false, From ef2efb808d89f5c1ea19e38c9b7513a27d503a89 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 13:55:12 -0600 Subject: [PATCH 18/30] Update antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml Co-authored-by: Sai Kishor Kothakota --- .../src/antipodal_gripper_action_controller_parameters.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml index a08d7596f8..dcf90ec613 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -48,6 +48,9 @@ antipodal_gripper_action_controller: type: double, description: "stall timeout", default_value: 1.0, + validation: { + gt_eq: [ 0.0 ] + }, } configure_max_effort_interface: { type: bool, From 4a995027dd970fefe67b01c01d380dd6d7f61b8d Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 14:22:32 -0600 Subject: [PATCH 19/30] change boolean interface parameter to string Signed-off-by: Paul Gesel --- ...tipodal_gripper_action_controller_impl.hpp | 12 ++--- ..._gripper_action_controller_parameters.yaml | 52 +++++++++---------- 2 files changed, 32 insertions(+), 32 deletions(-) diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index aa05e10486..eb660739c7 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -109,7 +109,7 @@ void GripperActionController::accepted_callback( // We use command_ for sharing command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; if ( - params_.configure_max_velocity_interface && !goal_handle->get_goal()->command.velocity.empty()) + !params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) { command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; } @@ -117,7 +117,7 @@ void GripperActionController::accepted_callback( { command_struct_.max_velocity_ = params_.max_velocity; } - if (params_.configure_max_effort_interface && !goal_handle->get_goal()->command.effort.empty()) + if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty()) { command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; } @@ -365,13 +365,13 @@ controller_interface::InterfaceConfiguration GripperActionController::command_interface_configuration() const { std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; - if (params_.configure_max_effort_interface) + if (!params_.max_effort_interface.empty()) { - names.push_back({params_.joint + "/set_gripper_max_effort"}); + names.push_back({params_.max_effort_interface}); } - if (params_.configure_max_velocity_interface) + if (!params_.max_velocity_interface.empty()) { - names.push_back({params_.joint + "/set_gripper_max_velocity"}); + names.push_back({params_.max_velocity_interface}); } return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml index dcf90ec613..21356cf180 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -18,22 +18,6 @@ antipodal_gripper_action_controller: gt_eq: [ 0.0 ] }, } - max_effort: { - type: double, - default_value: 10.0, - description: "Default effort used for grasping (Newtons)", - validation: { - gt_eq: [ 0.0 ] - }, - } - max_velocity: { - type: double, - default_value: 0.1, - description: "Default target velocity used for grasping (meters/second)", - validation: { - gt_eq: [ 0.0 ] - }, - } allow_stalling: { type: bool, description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", @@ -48,17 +32,33 @@ antipodal_gripper_action_controller: type: double, description: "stall timeout", default_value: 1.0, - validation: { - gt_eq: [ 0.0 ] + validation: { + gt_eq: [ 0.0 ] }, } - configure_max_effort_interface: { - type: bool, - description: "Controller will claim the {joint}/gripper_effort interface if true.", - default_value: false, + max_effort_interface: { + type: string, + description: "Controller will claim the specified effort interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", } - configure_max_velocity_interface: { - type: bool, - description: "Controller will claim the {joint}/gripper_speed interface if true.", - default_value: false, + max_effort: { + type: double, + default_value: 10.0, + description: "Default effort used for grasping (Newtons)", + validation: { + gt_eq: [ 0.0 ] + }, + } + max_velocity_interface: { + type: string, + description: "Controller will claim the speed specified interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_velocity: { + type: double, + default_value: 0.1, + description: "Default target velocity used for grasping (meters/second)", + validation: { + gt_eq: [ 0.0 ] + }, } From 5f7c2d8b32e127892829f97e2c9f82921351f0a7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 14:22:50 -0600 Subject: [PATCH 20/30] Update antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml Co-authored-by: Sai Kishor Kothakota --- .../src/antipodal_gripper_action_controller_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml index dcf90ec613..e3513856b4 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -9,7 +9,7 @@ antipodal_gripper_action_controller: } joint: { type: string, - default_value: "", + read_only: true, } goal_tolerance: { type: double, From 24160ca213d9fbdb5be41a8dc1dafb8d70c4c8f7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 14:28:45 -0600 Subject: [PATCH 21/30] Update antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml Co-authored-by: Sai Kishor Kothakota --- .../src/antipodal_gripper_action_controller_parameters.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml index e3513856b4..984bf9d576 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -3,6 +3,7 @@ antipodal_gripper_action_controller: type: double, default_value: 20.0, description: "Hz", + read_only: true, validation: { gt_eq: [ 0.1 ] }, From 7145368a3784737b56d0218a17a20e12b0739710 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 15:06:32 -0600 Subject: [PATCH 22/30] add detail userdoc and configurable state interface Signed-off-by: Paul Gesel --- antipodal_gripper_controller/doc/userdoc.rst | 5 +++++ .../antipodal_gripper_action_controller_impl.hpp | 13 +++++++------ ...ipodal_gripper_action_controller_parameters.yaml | 4 ++++ 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/antipodal_gripper_controller/doc/userdoc.rst b/antipodal_gripper_controller/doc/userdoc.rst index 35e426c4e7..8c76db6227 100644 --- a/antipodal_gripper_controller/doc/userdoc.rst +++ b/antipodal_gripper_controller/doc/userdoc.rst @@ -6,6 +6,11 @@ Antipodal Gripper Action Controller ----------------------------------- Controller for executing a ``AntipodalGripperCommand`` action for simple antipodal grippers. +This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. +By default, the controller will only claim the `{joint}/position` interface for control. +The velocity and effort interfaces can be optionally claimed by setting the `max_velocity_interface` and `max_effort_interface` parameter, respectively. +By default, the controller will try to claim position and velocity state interfaces. +The claimed state interfaces can be configured by setting the `state_interfaces` parameter. Parameters ^^^^^^^^^^^ diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index eb660739c7..fac6be6974 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -108,8 +108,7 @@ void GripperActionController::accepted_callback( // This is the non-realtime command_struct // We use command_ for sharing command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; - if ( - !params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) + if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) { command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; } @@ -380,10 +379,12 @@ GripperActionController::command_interface_configuration() const controller_interface::InterfaceConfiguration GripperActionController::state_interface_configuration() const { - return { - controller_interface::interface_configuration_type::INDIVIDUAL, - {params_.joint + "/" + hardware_interface::HW_IF_POSITION, - params_.joint + "/" + hardware_interface::HW_IF_VELOCITY}}; + std::vector interface_names; + for (const auto & interface : params_.state_interfaces) + { + interface_names.push_back(params_.joint + "/" + interface); + } + return {controller_interface::interface_configuration_type::INDIVIDUAL, interface_names}; } GripperActionController::GripperActionController() diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml index 03aa65742a..5c3fcf8e1e 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml @@ -12,6 +12,10 @@ antipodal_gripper_action_controller: type: string, read_only: true, } + state_interfaces: { + type: string_array, + default_value: [position, velocity], + } goal_tolerance: { type: double, default_value: 0.01, From 347b235fc81d682a312febc09f19ed04867b35a3 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 15:15:05 -0600 Subject: [PATCH 23/30] update cmake Signed-off-by: Paul Gesel --- antipodal_gripper_controller/CMakeLists.txt | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/antipodal_gripper_controller/CMakeLists.txt b/antipodal_gripper_controller/CMakeLists.txt index af5ca08921..46de281e83 100644 --- a/antipodal_gripper_controller/CMakeLists.txt +++ b/antipodal_gripper_controller/CMakeLists.txt @@ -1,13 +1,9 @@ cmake_minimum_required(VERSION 3.16) project(antipodal_gripper_controller LANGUAGES CXX) -if(APPLE OR WIN32) - message(WARNING "Antipodal gripper controllers are not available on OSX or Windows") - return() -endif() - if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS From deff36aec8809d8a79a1df4ee6e9f38d2b1933e7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 15:54:11 -0600 Subject: [PATCH 24/30] fix test Signed-off-by: Paul Gesel --- .../test/test_antipodal_gripper_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp index 5633138213..74f6d2e684 100644 --- a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp +++ b/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp @@ -48,7 +48,8 @@ void GripperControllerTest::TearDown() { controller_.reset(nullptr); } void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller"); + const auto result = + controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; From 6b44ba8ebfbe73e00bb0893a2300135b2d9cfcdf Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 2 Apr 2024 16:47:33 -0600 Subject: [PATCH 25/30] rename controller Signed-off-by: Paul Gesel --- antipodal_gripper_controller/CMakeLists.txt | 84 ------------------- .../ros_control_plugins.xml | 10 --- .../gripper_action_controller_impl.hpp | 2 +- parallel_gripper_controller/CMakeLists.txt | 84 +++++++++++++++++++ .../doc/userdoc.rst | 8 +- .../parallel_gripper_action_controller.hpp | 26 +++--- ...arallel_gripper_action_controller_impl.hpp | 18 ++-- .../visibility_control.hpp | 6 +- .../package.xml | 4 +- .../ros_control_plugins.xml | 10 +++ .../parallel_gripper_action_controller.cpp | 4 +- ..._gripper_action_controller_parameters.yaml | 2 +- ...oad_parallel_gripper_action_controller.cpp | 0 .../test/test_parallel_gripper_controller.cpp | 4 +- .../test/test_parallel_gripper_controller.hpp | 10 +-- 15 files changed, 136 insertions(+), 136 deletions(-) delete mode 100644 antipodal_gripper_controller/CMakeLists.txt delete mode 100644 antipodal_gripper_controller/ros_control_plugins.xml create mode 100644 parallel_gripper_controller/CMakeLists.txt rename {antipodal_gripper_controller => parallel_gripper_controller}/doc/userdoc.rst (79%) rename antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp => parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp (86%) rename antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp => parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp (95%) rename {antipodal_gripper_controller/include/antipodal_gripper_controller => parallel_gripper_controller/include/parallel_gripper_controller}/visibility_control.hpp (92%) rename {antipodal_gripper_controller => parallel_gripper_controller}/package.xml (89%) create mode 100644 parallel_gripper_controller/ros_control_plugins.xml rename antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp => parallel_gripper_controller/src/parallel_gripper_action_controller.cpp (84%) rename antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml => parallel_gripper_controller/src/parallel_gripper_action_controller_parameters.yaml (97%) rename antipodal_gripper_controller/test/test_load_antipodal_gripper_action_controller.cpp => parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp (100%) rename antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp => parallel_gripper_controller/test/test_parallel_gripper_controller.cpp (97%) rename antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp => parallel_gripper_controller/test/test_parallel_gripper_controller.hpp (84%) diff --git a/antipodal_gripper_controller/CMakeLists.txt b/antipodal_gripper_controller/CMakeLists.txt deleted file mode 100644 index 46de281e83..0000000000 --- a/antipodal_gripper_controller/CMakeLists.txt +++ /dev/null @@ -1,84 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(antipodal_gripper_controller LANGUAGES CXX) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_action - realtime_tools -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -generate_parameter_library(antipodal_gripper_action_controller_parameters - src/antipodal_gripper_action_controller_parameters.yaml -) - -add_library(antipodal_gripper_action_controller SHARED - src/antipodal_gripper_action_controller.cpp -) -target_compile_features(antipodal_gripper_action_controller PUBLIC cxx_std_17) -target_include_directories(antipodal_gripper_action_controller PUBLIC - $ - $ -) -target_link_libraries(antipodal_gripper_action_controller PUBLIC - antipodal_gripper_action_controller_parameters -) -ament_target_dependencies(antipodal_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_load_antipodal_gripper_action_controllers - test/test_load_antipodal_gripper_action_controller.cpp - ) - ament_target_dependencies(test_load_antipodal_gripper_action_controllers - controller_manager - ros2_control_test_assets - ) - - ament_add_gmock(test_antipodal_gripper_controller - test/test_antipodal_gripper_controller.cpp - ) - target_link_libraries(test_antipodal_gripper_controller - antipodal_gripper_action_controller - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/antipodal_gripper_action_controller -) -install( - TARGETS - antipodal_gripper_action_controller - antipodal_gripper_action_controller_parameters - EXPORT export_antipodal_gripper_action_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - INCLUDES DESTINATION include -) - -ament_export_targets(export_antipodal_gripper_action_controller HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/antipodal_gripper_controller/ros_control_plugins.xml b/antipodal_gripper_controller/ros_control_plugins.xml deleted file mode 100644 index 9c20e14360..0000000000 --- a/antipodal_gripper_controller/ros_control_plugins.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 76fb14fcd9..2c115091af 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -44,7 +44,7 @@ controller_interface::CallbackReturn GripperActionController: get_node()->get_logger(), "[Deprecated]: the `position_controllers/GripperActionController` and " "`effort_controllers::GripperActionController` controllers are replaced by " - "'antipodal_gripper_controllers/GripperActionController' controller"); + "'parallel_gripper_controllers/GripperActionController' controller"); try { param_listener_ = std::make_shared(get_node()); diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..34bd6d93ee --- /dev/null +++ b/parallel_gripper_controller/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.16) +project(parallel_gripper_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(parallel_gripper_action_controller_parameters + src/parallel_gripper_action_controller_parameters.yaml +) + +add_library(parallel_gripper_action_controller SHARED + src/parallel_gripper_action_controller.cpp +) +target_compile_features(parallel_gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(parallel_gripper_action_controller PUBLIC + $ + $ +) +target_link_libraries(parallel_gripper_action_controller PUBLIC + parallel_gripper_action_controller_parameters +) +ament_target_dependencies(parallel_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ) + ament_target_dependencies(test_load_parallel_gripper_action_controllers + controller_manager + ros2_control_test_assets + ) + + ament_add_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ) + target_link_libraries(test_parallel_gripper_controller + parallel_gripper_action_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/parallel_gripper_action_controller +) +install( + TARGETS + parallel_gripper_action_controller + parallel_gripper_action_controller_parameters + EXPORT export_parallel_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_parallel_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/antipodal_gripper_controller/doc/userdoc.rst b/parallel_gripper_controller/doc/userdoc.rst similarity index 79% rename from antipodal_gripper_controller/doc/userdoc.rst rename to parallel_gripper_controller/doc/userdoc.rst index 8c76db6227..be32591c75 100644 --- a/antipodal_gripper_controller/doc/userdoc.rst +++ b/parallel_gripper_controller/doc/userdoc.rst @@ -1,11 +1,11 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/antipodal_gripper_controller/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/parallel_gripper_controller/doc/userdoc.rst -.. _antipodal_gripper_controller_userdoc: +.. _parallel_gripper_controller_userdoc: -Antipodal Gripper Action Controller +Parallel Gripper Action Controller ----------------------------------- -Controller for executing a ``AntipodalGripperCommand`` action for simple antipodal grippers. +Controller for executing a ``ParallelGripperCommand`` action for simple parallel grippers. This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. By default, the controller will only claim the `{joint}/position` interface for control. The velocity and effort interfaces can be optionally claimed by setting the `max_velocity_interface` and `max_effort_interface` parameter, respectively. diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp similarity index 86% rename from antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp rename to parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index 45b7697528..f8f4df2492 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -14,8 +14,8 @@ /// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian -#ifndef ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ -#define ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ +#ifndef PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ // C++ standard #include @@ -27,13 +27,13 @@ #include "rclcpp/rclcpp.hpp" // ROS messages -#include "control_msgs/action/antipodal_gripper_command.hpp" +#include "control_msgs/action/parallel_gripper_command.hpp" // rclcpp_action #include "rclcpp_action/create_server.hpp" // ros_controls -#include "antipodal_gripper_controller/visibility_control.hpp" +#include "parallel_gripper_controller/visibility_control.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" @@ -41,12 +41,12 @@ #include "realtime_tools/realtime_server_goal_handle.h" // Project -#include "antipodal_gripper_action_controller_parameters.hpp" +#include "parallel_gripper_action_controller_parameters.hpp" -namespace antipodal_gripper_action_controller +namespace parallel_gripper_action_controller { /** - * \brief Controller for executing a `AntipodalGripperCommand` action. + * \brief Controller for executing a `ParallelGripperCommand` action. * * \tparam HardwareInterface Controller hardware interface. Currently \p * hardware_interface::HW_IF_POSITION and \p @@ -107,12 +107,12 @@ class GripperActionController : public controller_interface::ControllerInterface Commands command_struct_, command_struct_rt_; protected: - using GripperCommandAction = control_msgs::action::AntipodalGripperCommand; + using GripperCommandAction = control_msgs::action::ParallelGripperCommand; using ActionServer = rclcpp_action::Server; using ActionServerPtr = ActionServer::SharedPtr; using GoalHandle = rclcpp_action::ServerGoalHandle; using RealtimeGoalHandle = - realtime_tools::RealtimeServerGoalHandle; + realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; @@ -136,7 +136,7 @@ class GripperActionController : public controller_interface::ControllerInterface RealtimeGoalHandleBuffer rt_active_goal_; ///< Container for the currently active action goal, if any. - control_msgs::action::AntipodalGripperCommand::Result::SharedPtr pre_alloc_result_; + control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; rclcpp::Duration action_monitor_period_; @@ -167,8 +167,8 @@ class GripperActionController : public controller_interface::ControllerInterface double current_velocity); }; -} // namespace antipodal_gripper_action_controller +} // namespace parallel_gripper_action_controller -#include "antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp" -#endif // ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ +#endif // PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp similarity index 95% rename from antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp rename to parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index fac6be6974..d247206125 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -14,16 +14,16 @@ /// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser -#ifndef ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ -#define ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#ifndef PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ -#include "antipodal_gripper_controller/antipodal_gripper_action_controller.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" #include #include #include -namespace antipodal_gripper_action_controller +namespace parallel_gripper_action_controller { void GripperActionController::preempt_active_goal() @@ -83,7 +83,7 @@ rclcpp_action::GoalResponse GripperActionController::goal_callback( { if (goal_handle->command.position.size() != 1) { - pre_alloc_result_ = std::make_shared(); + pre_alloc_result_ = std::make_shared(); pre_alloc_result_->state.position.resize(1); pre_alloc_result_->state.effort.resize(1); RCLCPP_ERROR( @@ -332,7 +332,7 @@ controller_interface::CallbackReturn GripperActionController::on_activate( command_.initRT(command_struct_); // Result - pre_alloc_result_ = std::make_shared(); + pre_alloc_result_ = std::make_shared(); pre_alloc_result_->state.position.resize(1); pre_alloc_result_->state.effort.resize(1); pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; @@ -340,7 +340,7 @@ controller_interface::CallbackReturn GripperActionController::on_activate( pre_alloc_result_->stalled = false; // Action interface - action_server_ = rclcpp_action::create_server( + action_server_ = rclcpp_action::create_server( get_node(), "~/gripper_cmd", std::bind( &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), @@ -393,6 +393,6 @@ GripperActionController::GripperActionController() { } -} // namespace antipodal_gripper_action_controller +} // namespace parallel_gripper_action_controller -#endif // ANTIPODAL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#endif // PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/visibility_control.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp similarity index 92% rename from antipodal_gripper_controller/include/antipodal_gripper_controller/visibility_control.hpp rename to parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp index 7e305c720d..63e5c8e1c7 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/visibility_control.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp @@ -19,8 +19,8 @@ * library cannot have, but the consuming code must have inorder to link. */ -#ifndef ANTIPODAL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ -#define ANTIPODAL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#ifndef PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -53,4 +53,4 @@ #define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE #endif -#endif // ANTIPODAL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#endif // PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/antipodal_gripper_controller/package.xml b/parallel_gripper_controller/package.xml similarity index 89% rename from antipodal_gripper_controller/package.xml rename to parallel_gripper_controller/package.xml index 8fe2ecdfa4..f189071617 100644 --- a/antipodal_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -3,9 +3,9 @@ href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> - antipodal_gripper_controller + parallel_gripper_controller 2.32.0 - The antipodal_gripper_controller package + The parallel_gripper_controller package Bence Magyar Apache License 2.0 diff --git a/parallel_gripper_controller/ros_control_plugins.xml b/parallel_gripper_controller/ros_control_plugins.xml new file mode 100644 index 0000000000..dcd9609ba7 --- /dev/null +++ b/parallel_gripper_controller/ros_control_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp similarity index 84% rename from antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp rename to parallel_gripper_controller/src/parallel_gripper_action_controller.cpp index f7b6c651a8..910686493d 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller.cpp +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp @@ -15,11 +15,11 @@ /// \author Sachin Chitta // Project -#include +#include #include #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - antipodal_gripper_action_controller::GripperActionController, + parallel_gripper_action_controller::GripperActionController, controller_interface::ControllerInterface) diff --git a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/parallel_gripper_action_controller_parameters.yaml similarity index 97% rename from antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml rename to parallel_gripper_controller/src/parallel_gripper_action_controller_parameters.yaml index 5c3fcf8e1e..9b0dbb8a05 100644 --- a/antipodal_gripper_controller/src/antipodal_gripper_action_controller_parameters.yaml +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller_parameters.yaml @@ -1,4 +1,4 @@ -antipodal_gripper_action_controller: +parallel_gripper_action_controller: action_monitor_rate: { type: double, default_value: 20.0, diff --git a/antipodal_gripper_controller/test/test_load_antipodal_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp similarity index 100% rename from antipodal_gripper_controller/test/test_load_antipodal_gripper_action_controller.cpp rename to parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp diff --git a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp similarity index 97% rename from antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp rename to parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index 74f6d2e684..d9a5ae958b 100644 --- a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -20,7 +20,7 @@ #include "gmock/gmock.h" -#include "test_antipodal_gripper_controller.hpp" +#include "test_parallel_gripper_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -29,7 +29,7 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; -using GripperCommandAction = control_msgs::action::AntipodalGripperCommand; +using GripperCommandAction = control_msgs::action::ParallelGripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; using testing::SizeIs; using testing::UnorderedElementsAre; diff --git a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp similarity index 84% rename from antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp rename to parallel_gripper_controller/test/test_parallel_gripper_controller.hpp index 7b3a7036f7..d46d8722eb 100644 --- a/antipodal_gripper_controller/test/test_antipodal_gripper_controller.hpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST_ANTIPODAL_GRIPPER_CONTROLLER_HPP_ -#define TEST_ANTIPODAL_GRIPPER_CONTROLLER_HPP_ +#ifndef TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ +#define TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ #include #include @@ -21,14 +21,14 @@ #include "gmock/gmock.h" -#include "antipodal_gripper_controller/antipodal_gripper_action_controller.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" namespace { // subclassing and friending so we can access member variables -class FriendGripperController : public antipodal_gripper_action_controller::GripperActionController +class FriendGripperController : public parallel_gripper_action_controller::GripperActionController { FRIEND_TEST(GripperControllerTest, CommandSuccessTest); }; @@ -63,4 +63,4 @@ class GripperControllerTest : public ::testing::Test } // anonymous namespace -#endif // TEST_ANTIPODAL_GRIPPER_CONTROLLER_HPP_ +#endif // TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ From 254ae0705e8854ec44a8e4e47777a5ddc3338310 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 8 Apr 2024 16:56:24 -0600 Subject: [PATCH 26/30] fix format and add control_msgs to repos file Signed-off-by: Paul Gesel --- parallel_gripper_controller/doc/userdoc.rst | 6 +++--- .../parallel_gripper_action_controller.hpp | 8 ++++---- .../parallel_gripper_action_controller_impl.hpp | 13 +++++++------ .../src/parallel_gripper_action_controller.cpp | 4 ++-- .../test/test_parallel_gripper_controller.hpp | 2 +- ros2_controllers-not-released.rolling.repos | 4 ---- ros2_controllers.rolling.repos | 4 ++-- 7 files changed, 19 insertions(+), 22 deletions(-) diff --git a/parallel_gripper_controller/doc/userdoc.rst b/parallel_gripper_controller/doc/userdoc.rst index be32591c75..5c04eb5f90 100644 --- a/parallel_gripper_controller/doc/userdoc.rst +++ b/parallel_gripper_controller/doc/userdoc.rst @@ -7,10 +7,10 @@ Parallel Gripper Action Controller Controller for executing a ``ParallelGripperCommand`` action for simple parallel grippers. This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. -By default, the controller will only claim the `{joint}/position` interface for control. -The velocity and effort interfaces can be optionally claimed by setting the `max_velocity_interface` and `max_effort_interface` parameter, respectively. +By default, the controller will only claim the ``{joint}/position`` interface for control. +The velocity and effort interfaces can be optionally claimed by setting the ``max_velocity_interface`` and ``max_effort_interface`` parameter, respectively. By default, the controller will try to claim position and velocity state interfaces. -The claimed state interfaces can be configured by setting the `state_interfaces` parameter. +The claimed state interfaces can be configured by setting the ``state_interfaces`` parameter. Parameters ^^^^^^^^^^^ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index f8f4df2492..d303990b89 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -14,8 +14,8 @@ /// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian -#ifndef PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ -#define PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ // C++ standard #include @@ -33,10 +33,10 @@ #include "rclcpp_action/create_server.hpp" // ros_controls -#include "parallel_gripper_controller/visibility_control.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "parallel_gripper_controller/visibility_control.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_server_goal_handle.h" @@ -171,4 +171,4 @@ class GripperActionController : public controller_interface::ControllerInterface #include "parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp" -#endif // PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_HPP_ +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index d247206125..61ca76b445 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -14,14 +14,15 @@ /// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser -#ifndef PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ -#define PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ -#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" - -#include #include #include +#include + +#include +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" namespace parallel_gripper_action_controller { @@ -395,4 +396,4 @@ GripperActionController::GripperActionController() } // namespace parallel_gripper_action_controller -#endif // PARALLEL_GRIPPER_CONTROLLER__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp index 910686493d..3a6d028ddf 100644 --- a/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp @@ -15,11 +15,11 @@ /// \author Sachin Chitta // Project -#include #include +#include #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - parallel_gripper_action_controller::GripperActionController, + parallel_gripper_action_controller::GripperActionController, controller_interface::ControllerInterface) diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp index d46d8722eb..8bc6ab954c 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -21,9 +21,9 @@ #include "gmock/gmock.h" -#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" namespace { diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 30366b5406..66352f4960 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -3,7 +3,3 @@ repositories: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git version: main - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: pr-add-gripper-velocity-target-main diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 8c20eccc96..c98c68034f 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -9,8 +9,8 @@ repositories: version: master control_msgs: type: git - url: https://github.com/ros-controls/control_msgs.git - version: master + url: https://github.com/pac48/control_msgs.git + version: pr-add-gripper-velocity-target-main control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git From 02aa67a3db7659b40bfa53a3e5fd7ffe534582d2 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 8 Apr 2024 17:01:15 -0600 Subject: [PATCH 27/30] fix parameter file name Signed-off-by: Paul Gesel --- parallel_gripper_controller/CMakeLists.txt | 2 +- ...arameters.yaml => gripper_action_controller_parameters.yaml} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename parallel_gripper_controller/src/{parallel_gripper_action_controller_parameters.yaml => gripper_action_controller_parameters.yaml} (100%) diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 34bd6d93ee..56aa623a6f 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -25,7 +25,7 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() generate_parameter_library(parallel_gripper_action_controller_parameters - src/parallel_gripper_action_controller_parameters.yaml + src/gripper_action_controller_parameters.yaml ) add_library(parallel_gripper_action_controller SHARED diff --git a/parallel_gripper_controller/src/parallel_gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml similarity index 100% rename from parallel_gripper_controller/src/parallel_gripper_action_controller_parameters.yaml rename to parallel_gripper_controller/src/gripper_action_controller_parameters.yaml From fcbe5d21d70be2ade24d4112be6b14c349be0ec0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 8 May 2024 20:27:06 +0100 Subject: [PATCH 28/30] use upstream control_msgs --- ros2_controllers.rolling.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index c98c68034f..8c20eccc96 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -9,8 +9,8 @@ repositories: version: master control_msgs: type: git - url: https://github.com/pac48/control_msgs.git - version: pr-add-gripper-velocity-target-main + url: https://github.com/ros-controls/control_msgs.git + version: master control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git From ebb347fc9751d90a2a6901e23ded91a325887e10 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 8 May 2024 23:13:08 +0200 Subject: [PATCH 29/30] Add controller to index --- doc/controllers_index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index d042fe79dd..a61de035a6 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -55,6 +55,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> From 2c74803671c869b1e2b7b4523e8dda350e7e109d Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 11 Jul 2024 06:51:28 -0700 Subject: [PATCH 30/30] Update parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp Co-authored-by: Bence Magyar --- .../parallel_gripper_action_controller_impl.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 61ca76b445..b6fccf1bbf 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -322,9 +322,6 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } } - // Hardware interface adapter - // hw_iface_adapter_.init(joint_command_interface_, speed_interface_, effort_interface_, - // get_node()); // Command - non RT version command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value();