Skip to content

Commit

Permalink
add deprecation warning + fix test
Browse files Browse the repository at this point in the history
Signed-off-by: Paul Gesel <[email protected]>
  • Loading branch information
pac48 committed Feb 1, 2024
1 parent 948f621 commit 66a9b7f
Show file tree
Hide file tree
Showing 6 changed files with 24 additions and 27 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,14 @@
#include "rclcpp/rclcpp.hpp"

// ROS messages
#include "control_msgs/action/gripper_command.hpp"
#include "control_msgs/action/gripper_command_velocity.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"
Expand Down
2 changes: 1 addition & 1 deletion antipodal_gripper_controller/ros_control_plugins.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<library path="gripper_action_controller">

<class name="antipodal_position_controllers/GripperActionController"
type="position_controllers::GripperActionController"
type="antipodal_position_controllers::GripperActionController"
base_class_type="controller_interface::ControllerInterface">
<description>
</description>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Original file line number Diff line number Diff line change
Expand Up @@ -34,31 +34,17 @@ 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()
{
Expand Down Expand Up @@ -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));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
};
Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_position_controllers/GripperActionController' controller");
try
{
param_listener_ = std::make_shared<ParamListener>(get_node());
Expand Down

0 comments on commit 66a9b7f

Please sign in to comment.