17 #ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
18 #define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
24 #include <hardware_interface/types/hardware_interface_type_values.hpp>
25 #include "parallel_gripper_controller/parallel_gripper_action_controller.hpp"
30 void GripperActionController::preempt_active_goal()
37 active_goal->setCanceled(std::make_shared<GripperCommandAction::Result>());
46 param_listener_ = std::make_shared<ParamListener>(get_node());
48 catch (
const std::exception & e)
50 fprintf(stderr,
"Exception thrown during init stage with message: %s \n", e.what());
51 return controller_interface::CallbackReturn::ERROR;
54 return controller_interface::CallbackReturn::SUCCESS;
58 const rclcpp::Time & ,
const rclcpp::Duration & )
60 command_struct_rt_ = *(command_.readFromRT());
62 const double current_position = joint_position_state_interface_->get().get_value();
63 const double current_velocity = joint_velocity_state_interface_->get().get_value();
64 const double error_position = command_struct_rt_.position_cmd_ - current_position;
66 check_for_success(get_node()->now(), error_position, current_position, current_velocity);
68 joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_);
69 if (speed_interface_.has_value())
71 speed_interface_->get().set_value(command_struct_rt_.max_velocity_);
73 if (effort_interface_.has_value())
75 effort_interface_->get().set_value(command_struct_rt_.max_effort_);
78 return controller_interface::return_type::OK;
81 rclcpp_action::GoalResponse GripperActionController::goal_callback(
82 const rclcpp_action::GoalUUID &, std::shared_ptr<const GripperCommandAction::Goal> goal_handle)
84 if (goal_handle->command.position.size() != 1)
86 pre_alloc_result_ = std::make_shared<control_msgs::action::ParallelGripperCommand::Result>();
87 pre_alloc_result_->state.position.resize(1);
88 pre_alloc_result_->state.effort.resize(1);
90 get_node()->get_logger(),
91 "Received action goal with wrong number of position values, expects 1, got %zu",
92 goal_handle->command.position.size());
93 return rclcpp_action::GoalResponse::REJECT;
96 RCLCPP_INFO(get_node()->get_logger(),
"Received & accepted new action goal");
97 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
100 void GripperActionController::accepted_callback(
101 std::shared_ptr<GoalHandle> goal_handle)
103 auto rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
106 preempt_active_goal();
110 command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0];
111 if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty())
113 command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0];
117 command_struct_.max_velocity_ = params_.max_velocity;
119 if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty())
121 command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0];
125 command_struct_.max_effort_ = params_.max_effort;
127 command_.writeFromNonRT(command_struct_);
129 pre_alloc_result_->reached_goal =
false;
130 pre_alloc_result_->stalled =
false;
137 goal_handle_timer_.reset();
140 goal_handle_timer_ = get_node()->create_wall_timer(
141 action_monitor_period_.to_chrono<std::chrono::nanoseconds>(),
142 std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
145 rclcpp_action::CancelResponse GripperActionController::cancel_callback(
146 const std::shared_ptr<GoalHandle> goal_handle)
148 RCLCPP_INFO(get_node()->get_logger(),
"Got request to cancel goal");
152 if (active_goal && active_goal->gh_ == goal_handle)
158 get_node()->get_logger(),
"Canceling active action goal because cancel callback received.");
161 auto action_res = std::make_shared<GripperCommandAction::Result>();
162 active_goal->setCanceled(action_res);
166 return rclcpp_action::CancelResponse::ACCEPT;
169 void GripperActionController::set_hold_position()
171 command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value();
172 command_struct_.max_effort_ = params_.max_effort;
173 command_struct_.max_velocity_ = params_.max_velocity;
174 command_.writeFromNonRT(command_struct_);
178 const rclcpp::Time & time,
double error_position,
double current_position,
179 double current_velocity)
187 if (fabs(error_position) < params_.goal_tolerance)
190 pre_alloc_result_->state.position[0] = current_position;
191 pre_alloc_result_->reached_goal =
true;
192 pre_alloc_result_->stalled =
false;
193 RCLCPP_DEBUG(get_node()->get_logger(),
"Successfully moved to goal.");
194 active_goal->setSucceeded(pre_alloc_result_);
199 if (fabs(current_velocity) > params_.stall_velocity_threshold)
206 pre_alloc_result_->state.position[0] = current_position;
207 pre_alloc_result_->reached_goal =
false;
208 pre_alloc_result_->stalled =
true;
210 if (params_.allow_stalling)
212 RCLCPP_DEBUG(get_node()->get_logger(),
"Stall detected moving to goal. Returning success.");
213 active_goal->setSucceeded(pre_alloc_result_);
217 RCLCPP_DEBUG(get_node()->get_logger(),
"Stall detected moving to goal. Aborting action!");
218 active_goal->setAborted(pre_alloc_result_);
225 controller_interface::CallbackReturn GripperActionController::on_configure(
226 const rclcpp_lifecycle::State &)
228 const auto logger = get_node()->get_logger();
229 if (!param_listener_)
231 RCLCPP_ERROR(get_node()->get_logger(),
"Error encountered during init");
232 return controller_interface::CallbackReturn::ERROR;
234 params_ = param_listener_->get_params();
237 action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate);
239 logger,
"Action status changes will be monitored at " << params_.action_monitor_rate <<
"Hz.");
242 if (params_.joint.empty())
244 RCLCPP_ERROR(logger,
"Joint name cannot be empty");
245 return controller_interface::CallbackReturn::ERROR;
247 RCLCPP_INFO(logger,
"Joint name is : %s", params_.joint.c_str());
249 return controller_interface::CallbackReturn::SUCCESS;
252 controller_interface::CallbackReturn GripperActionController::on_activate(
253 const rclcpp_lifecycle::State &)
255 auto command_interface_it = std::find_if(
256 command_interfaces_.begin(), command_interfaces_.end(),
258 { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; });
259 if (command_interface_it == command_interfaces_.end())
262 get_node()->get_logger(),
264 return controller_interface::CallbackReturn::ERROR;
266 if (command_interface_it->get_prefix_name() != params_.joint)
269 get_node()->get_logger(),
"Command interface is different than joint name `"
270 << command_interface_it->get_prefix_name() <<
"` != `"
271 << params_.joint <<
"`");
272 return controller_interface::CallbackReturn::ERROR;
274 const auto position_state_interface_it = std::find_if(
275 state_interfaces_.begin(), state_interfaces_.end(),
277 { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; });
278 if (position_state_interface_it == state_interfaces_.end())
280 RCLCPP_ERROR(get_node()->get_logger(),
"Expected 1 position state interface");
281 return controller_interface::CallbackReturn::ERROR;
283 if (position_state_interface_it->get_prefix_name() != params_.joint)
286 get_node()->get_logger(),
"Position state interface is different than joint name `"
287 << position_state_interface_it->get_prefix_name() <<
"` != `"
288 << params_.joint <<
"`");
289 return controller_interface::CallbackReturn::ERROR;
291 const auto velocity_state_interface_it = std::find_if(
292 state_interfaces_.begin(), state_interfaces_.end(),
294 { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; });
295 if (velocity_state_interface_it == state_interfaces_.end())
297 RCLCPP_ERROR(get_node()->get_logger(),
"Expected 1 velocity state interface");
298 return controller_interface::CallbackReturn::ERROR;
300 if (velocity_state_interface_it->get_prefix_name() != params_.joint)
303 get_node()->get_logger(),
"Velocity command interface is different than joint name `"
304 << velocity_state_interface_it->get_prefix_name() <<
"` != `"
305 << params_.joint <<
"`");
306 return controller_interface::CallbackReturn::ERROR;
309 joint_command_interface_ = *command_interface_it;
310 joint_position_state_interface_ = *position_state_interface_it;
311 joint_velocity_state_interface_ = *velocity_state_interface_it;
313 for (
auto & interface : command_interfaces_)
315 if (interface.get_interface_name() ==
"set_gripper_max_effort")
317 effort_interface_ = interface;
319 else if (interface.get_interface_name() ==
"set_gripper_max_velocity")
321 speed_interface_ = interface;
326 command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value();
327 command_struct_.max_effort_ = params_.max_effort;
328 command_struct_.max_velocity_ = params_.max_velocity;
329 command_.initRT(command_struct_);
332 pre_alloc_result_ = std::make_shared<control_msgs::action::ParallelGripperCommand::Result>();
333 pre_alloc_result_->state.position.resize(1);
334 pre_alloc_result_->state.effort.resize(1);
335 pre_alloc_result_->state.position[0] = command_struct_.position_cmd_;
336 pre_alloc_result_->reached_goal =
false;
337 pre_alloc_result_->stalled =
false;
340 action_server_ = rclcpp_action::create_server<control_msgs::action::ParallelGripperCommand>(
341 get_node(),
"~/gripper_cmd",
343 &GripperActionController::goal_callback,
this, std::placeholders::_1, std::placeholders::_2),
344 std::bind(&GripperActionController::cancel_callback,
this, std::placeholders::_1),
345 std::bind(&GripperActionController::accepted_callback,
this, std::placeholders::_1));
347 return controller_interface::CallbackReturn::SUCCESS;
350 controller_interface::CallbackReturn GripperActionController::on_deactivate(
351 const rclcpp_lifecycle::State &)
353 joint_command_interface_ = std::nullopt;
354 joint_position_state_interface_ = std::nullopt;
355 joint_velocity_state_interface_ = std::nullopt;
357 return controller_interface::CallbackReturn::SUCCESS;
364 if (!params_.max_effort_interface.empty())
366 names.push_back({params_.max_effort_interface});
368 if (!params_.max_velocity_interface.empty())
370 names.push_back({params_.max_velocity_interface});
373 return {controller_interface::interface_configuration_type::INDIVIDUAL, names};
379 std::vector<std::string> interface_names;
380 for (
const auto & interface : params_.state_interfaces)
382 interface_names.push_back(params_.joint +
"/" + interface);
384 return {controller_interface::interface_configuration_type::INDIVIDUAL, interface_names};
387 GripperActionController::GripperActionController()
388 : controller_interface::ControllerInterface(),
389 action_monitor_period_(rclcpp::Duration::from_seconds(0))
virtual CONTROLLER_INTERFACE_PUBLIC void release_interfaces()
Method that releases the Loaned interfaces from the controller.
Definition: controller_interface_base.cpp:103
Definition: loaned_command_interface.hpp:27
Definition: loaned_state_interface.hpp:27
GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: parallel_gripper_action_controller_impl.hpp:57
double computed_command_
Computed command.
Definition: parallel_gripper_action_controller.hpp:160
GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
command_interface_configuration This controller requires the position command interfaces for the cont...
Definition: parallel_gripper_action_controller_impl.hpp:361
rclcpp::Time last_movement_time_
Store stall time.
Definition: parallel_gripper_action_controller.hpp:159
GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
command_interface_configuration This controller requires the position and velocity state interfaces f...
Definition: parallel_gripper_action_controller_impl.hpp:377
RealtimeGoalHandleBuffer rt_active_goal_
Container for the currently active action goal, if any.
Definition: parallel_gripper_action_controller.hpp:138
GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: parallel_gripper_action_controller_impl.hpp:42
void check_for_success(const rclcpp::Time &time, double error_position, double current_position, double current_velocity)
Check for success and publish appropriate result and feedback.
Definition: parallel_gripper_action_controller_impl.hpp:177
constexpr char HW_IF_POSITION[]
Constant defining position interface name.
Definition: hardware_interface_type_values.hpp:21
Definition: parallel_gripper_action_controller.hpp:47
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:56