17 #ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
18 #define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
20 #include "gripper_controllers/gripper_action_controller.hpp"
27 template <const
char * HardwareInterface>
28 void GripperActionController<HardwareInterface>::preempt_active_goal()
31 const auto active_goal = *rt_active_goal_.readFromNonRT();
35 active_goal->setCanceled(std::make_shared<GripperCommandAction::Result>());
36 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
40 template <const
char * HardwareInterface>
44 get_node()->get_logger(),
45 "[Deprecated]: the `position_controllers/GripperActionController` and "
46 "`effort_controllers::GripperActionController` controllers are replaced by "
47 "'parallel_gripper_controllers/GripperActionController' controller");
50 param_listener_ = std::make_shared<ParamListener>(get_node());
51 params_ = param_listener_->get_params();
53 catch (
const std::exception & e)
55 fprintf(stderr,
"Exception thrown during init stage with message: %s \n", e.what());
56 return controller_interface::CallbackReturn::ERROR;
59 return controller_interface::CallbackReturn::SUCCESS;
62 template <const
char * HardwareInterface>
64 const rclcpp::Time & ,
const rclcpp::Duration & )
66 command_struct_rt_ = *(command_.readFromRT());
68 const double current_position = joint_position_state_interface_->get().get_value();
69 const double current_velocity = joint_velocity_state_interface_->get().get_value();
71 const double error_position = command_struct_rt_.position_ - current_position;
72 const double error_velocity = -current_velocity;
74 check_for_success(get_node()->now(), error_position, current_position, current_velocity);
77 computed_command_ = hw_iface_adapter_.updateCommand(
78 command_struct_rt_.position_, 0.0, error_position, error_velocity,
79 command_struct_rt_.max_effort_);
80 return controller_interface::return_type::OK;
83 template <const
char * HardwareInterface>
85 const rclcpp_action::GoalUUID &, std::shared_ptr<const GripperCommandAction::Goal>)
87 RCLCPP_INFO(get_node()->get_logger(),
"Received & accepted new action goal");
88 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
91 template <const
char * HardwareInterface>
92 void GripperActionController<HardwareInterface>::accepted_callback(
93 std::shared_ptr<GoalHandle> goal_handle)
95 auto rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
98 preempt_active_goal();
102 command_struct_.position_ = goal_handle->get_goal()->command.position;
103 command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort;
104 command_.writeFromNonRT(command_struct_);
106 pre_alloc_result_->reached_goal =
false;
107 pre_alloc_result_->stalled =
false;
109 last_movement_time_ = get_node()->now();
111 rt_active_goal_.writeFromNonRT(rt_goal);
114 goal_handle_timer_.reset();
117 goal_handle_timer_ = get_node()->create_wall_timer(
118 action_monitor_period_.to_chrono<std::chrono::nanoseconds>(),
119 std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
122 template <const
char * HardwareInterface>
123 rclcpp_action::CancelResponse GripperActionController<HardwareInterface>::cancel_callback(
124 const std::shared_ptr<GoalHandle> goal_handle)
126 RCLCPP_INFO(get_node()->get_logger(),
"Got request to cancel goal");
129 const auto active_goal = *rt_active_goal_.readFromNonRT();
130 if (active_goal && active_goal->gh_ == goal_handle)
136 get_node()->get_logger(),
"Canceling active action goal because cancel callback received.");
139 auto action_res = std::make_shared<GripperCommandAction::Result>();
140 active_goal->setCanceled(action_res);
142 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
144 return rclcpp_action::CancelResponse::ACCEPT;
147 template <const
char * HardwareInterface>
148 void GripperActionController<HardwareInterface>::set_hold_position()
150 command_struct_.position_ = joint_position_state_interface_->get().get_value();
151 command_struct_.max_effort_ = params_.max_effort;
152 command_.writeFromNonRT(command_struct_);
155 template <const
char * HardwareInterface>
157 const rclcpp::Time & time,
double error_position,
double current_position,
158 double current_velocity)
160 const auto active_goal = *rt_active_goal_.readFromNonRT();
166 if (fabs(error_position) < params_.goal_tolerance)
168 pre_alloc_result_->effort = computed_command_;
169 pre_alloc_result_->position = current_position;
170 pre_alloc_result_->reached_goal =
true;
171 pre_alloc_result_->stalled =
false;
172 RCLCPP_DEBUG(get_node()->get_logger(),
"Successfully moved to goal.");
173 active_goal->setSucceeded(pre_alloc_result_);
174 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
178 if (fabs(current_velocity) > params_.stall_velocity_threshold)
180 last_movement_time_ = time;
182 else if ((time - last_movement_time_).seconds() > params_.stall_timeout)
184 pre_alloc_result_->effort = computed_command_;
185 pre_alloc_result_->position = current_position;
186 pre_alloc_result_->reached_goal =
false;
187 pre_alloc_result_->stalled =
true;
189 if (params_.allow_stalling)
191 RCLCPP_DEBUG(get_node()->get_logger(),
"Stall detected moving to goal. Returning success.");
192 active_goal->setSucceeded(pre_alloc_result_);
196 RCLCPP_DEBUG(get_node()->get_logger(),
"Stall detected moving to goal. Aborting action!");
197 active_goal->setAborted(pre_alloc_result_);
199 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
204 template <const
char * HardwareInterface>
206 const rclcpp_lifecycle::State &)
208 const auto logger = get_node()->get_logger();
209 if (!param_listener_)
211 RCLCPP_ERROR(get_node()->get_logger(),
"Error encountered during init");
212 return controller_interface::CallbackReturn::ERROR;
214 params_ = param_listener_->get_params();
217 action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate);
219 logger,
"Action status changes will be monitored at " << params_.action_monitor_rate <<
"Hz.");
222 if (params_.joint.empty())
224 RCLCPP_ERROR(logger,
"Joint name cannot be empty");
225 return controller_interface::CallbackReturn::ERROR;
228 return controller_interface::CallbackReturn::SUCCESS;
230 template <const
char * HardwareInterface>
231 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_activate(
232 const rclcpp_lifecycle::State &)
234 auto command_interface_it = std::find_if(
235 command_interfaces_.begin(), command_interfaces_.end(),
237 { return command_interface.get_interface_name() == HardwareInterface; });
238 if (command_interface_it == command_interfaces_.end())
241 get_node()->get_logger(),
"Expected 1 " << HardwareInterface <<
" command interface");
242 return controller_interface::CallbackReturn::ERROR;
244 if (command_interface_it->get_prefix_name() != params_.joint)
247 get_node()->get_logger(),
"Command interface is different than joint name `"
248 << command_interface_it->get_prefix_name() <<
"` != `"
249 << params_.joint <<
"`");
250 return controller_interface::CallbackReturn::ERROR;
252 const auto position_state_interface_it = std::find_if(
253 state_interfaces_.begin(), state_interfaces_.end(),
255 { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; });
256 if (position_state_interface_it == state_interfaces_.end())
258 RCLCPP_ERROR(get_node()->get_logger(),
"Expected 1 position state interface");
259 return controller_interface::CallbackReturn::ERROR;
261 if (position_state_interface_it->get_prefix_name() != params_.joint)
264 get_node()->get_logger(),
"Position state interface is different than joint name `"
265 << position_state_interface_it->get_prefix_name() <<
"` != `"
266 << params_.joint <<
"`");
267 return controller_interface::CallbackReturn::ERROR;
269 const auto velocity_state_interface_it = std::find_if(
270 state_interfaces_.begin(), state_interfaces_.end(),
272 { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; });
273 if (velocity_state_interface_it == state_interfaces_.end())
275 RCLCPP_ERROR(get_node()->get_logger(),
"Expected 1 velocity state interface");
276 return controller_interface::CallbackReturn::ERROR;
278 if (velocity_state_interface_it->get_prefix_name() != params_.joint)
281 get_node()->get_logger(),
"Velocity command interface is different than joint name `"
282 << velocity_state_interface_it->get_prefix_name() <<
"` != `"
283 << params_.joint <<
"`");
284 return controller_interface::CallbackReturn::ERROR;
287 joint_command_interface_ = *command_interface_it;
288 joint_position_state_interface_ = *position_state_interface_it;
289 joint_velocity_state_interface_ = *velocity_state_interface_it;
292 hw_iface_adapter_.init(joint_command_interface_, get_node());
295 command_struct_.position_ = joint_position_state_interface_->get().get_value();
296 command_struct_.max_effort_ = params_.max_effort;
297 command_.initRT(command_struct_);
300 pre_alloc_result_ = std::make_shared<control_msgs::action::GripperCommand::Result>();
301 pre_alloc_result_->position = command_struct_.position_;
302 pre_alloc_result_->reached_goal =
false;
303 pre_alloc_result_->stalled =
false;
306 action_server_ = rclcpp_action::create_server<control_msgs::action::GripperCommand>(
307 get_node(),
"~/gripper_cmd",
309 &GripperActionController::goal_callback,
this, std::placeholders::_1, std::placeholders::_2),
310 std::bind(&GripperActionController::cancel_callback,
this, std::placeholders::_1),
311 std::bind(&GripperActionController::accepted_callback,
this, std::placeholders::_1));
313 return controller_interface::CallbackReturn::SUCCESS;
316 template <const
char * HardwareInterface>
317 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_deactivate(
318 const rclcpp_lifecycle::State &)
320 joint_command_interface_ = std::nullopt;
321 joint_position_state_interface_ = std::nullopt;
322 joint_velocity_state_interface_ = std::nullopt;
323 release_interfaces();
324 return controller_interface::CallbackReturn::SUCCESS;
327 template <const
char * HardwareInterface>
332 controller_interface::interface_configuration_type::INDIVIDUAL,
333 {params_.joint +
"/" + HardwareInterface}};
336 template <const
char * HardwareInterface>
341 controller_interface::interface_configuration_type::INDIVIDUAL,
346 template <const
char * HardwareInterface>
348 : controller_interface::ControllerInterface(),
349 action_monitor_period_(rclcpp::Duration::from_seconds(0))
Controller for executing a gripper command action for simple single-dof grippers.
Definition: gripper_action_controller.hpp:61
GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: gripper_action_controller_impl.hpp:41
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: gripper_action_controller_impl.hpp:338
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: gripper_action_controller_impl.hpp:156
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: gripper_action_controller_impl.hpp:329
GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: gripper_action_controller_impl.hpp:63
Definition: loaned_command_interface.hpp:30
Definition: loaned_state_interface.hpp:29
Definition: gripper_action_controller.hpp:50
constexpr char HW_IF_VELOCITY[]
Constant defining velocity interface name.
Definition: hardware_interface_type_values.hpp:23
constexpr char HW_IF_POSITION[]
Constant defining position interface name.
Definition: hardware_interface_type_values.hpp:21
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58