From 4f994607e68b32cf840dcb47bf79773610a5f928 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 1 Feb 2024 16:09:28 -0700 Subject: [PATCH] add deprecation warning + fix test Signed-off-by: Paul Gesel --- .../antipodal_gripper_action_controller.hpp | 10 +++---- ...tipodal_gripper_action_controller_impl.hpp | 25 +++++++++++------- .../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, 45 insertions(+), 43 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..d3faba6dce 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,15 @@ 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(); + 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 +102,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 { @@ -322,13 +327,13 @@ 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_->position = 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());