add deprecation warning + fix test

Signed-off-by: Paul Gesel <[email protected]>

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<GripperCommandAction>;
   using ActionServerPtr = ActionServer::SharedPtr;
   using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
   using RealtimeGoalHandle =
-    realtime_tools::RealtimeServerGoalHandle<control_msgs::action::GripperCommand>;
+    realtime_tools::RealtimeServerGoalHandle<control_msgs::action::AntipodalGripperCommand>;
   using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
   using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
 
@@ -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_;
 

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 GripperCommandAction::Goal>)
+  const rclcpp_action::GoalUUID &, std::shared_ptr<const GripperCommandAction::Goal> goal_handle)
 {
+
+  if (goal_handle->command.position.size()!= 1){
+    pre_alloc_result_ = std::make_shared<control_msgs::action::AntipodalGripperCommand::Result>();
+    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<control_msgs::action::GripperCommand::Result>();
+  pre_alloc_result_ = std::make_shared<control_msgs::action::AntipodalGripperCommand::Result>();
   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<control_msgs::action::GripperCommand>(
+  action_server_ = rclcpp_action::create_server<control_msgs::action::AntipodalGripperCommand>(
     get_node(), "~/gripper_cmd",
     std::bind(
       &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2),

antipodal_gripper_controller/ros_control_plugins.xml

@@ -1,7 +1,7 @@
-<library path="gripper_action_controller">
+<library path="antipodal_gripper_action_controller">
 
-  <class name="antipodal_position_controllers/GripperActionController"
-         type="position_controllers::GripperActionController"
+  <class name="antipodal_gripper_action_controller/GripperActionController"
+         type="antipodal_gripper_action_controller::GripperActionController"
          base_class_type="controller_interface::ControllerInterface">
     <description>
     </description>

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)
+PLUGINLIB_EXPORT_CLASS(
+  antipodal_gripper_action_controller::GripperActionController,
+  controller_interface::ControllerInterface)

antipodal_gripper_controller/test/test_antipodal_gripper_controller.cpp

@@ -29,36 +29,22 @@
 
 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<GripperCommandAction>;
 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()
 {
   // initialize controller
   controller_ = std::make_unique<FriendGripperController>();
 }
 
-
-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));

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<double> joint_states_ = {1.1, 2.1};
   std::vector<double> 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

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -40,6 +40,11 @@ void GripperActionController<HardwareInterface>::preempt_active_goal()
 template <const char * HardwareInterface>
 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::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<ParamListener>(get_node());