15 #include "angles/angles.h"
16 #include "nav2_ros_common/rate.hpp"
17 #include "nav2_util/robot_utils.hpp"
18 #include "opennav_docking/docking_server.hpp"
19 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
20 #include "tf2/utils.hpp"
22 using namespace std::chrono_literals;
23 using rcl_interfaces::msg::ParameterType;
24 using std::placeholders::_1;
26 namespace opennav_docking
29 DockingServer::DockingServer(
const rclcpp::NodeOptions & options)
30 : nav2::LifecycleNode(
"docking_server",
"", options)
32 RCLCPP_INFO(get_logger(),
"Creating %s", get_name());
38 RCLCPP_INFO(get_logger(),
"Configuring %s", get_name());
40 param_handler_ = std::make_unique<ParameterHandler>(
42 params_ = param_handler_->getParams();
44 vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node,
"cmd_vel");
45 tf2_buffer_ = nav2::create_transform_buffer(node);
48 odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, params_->odom_duration,
52 docking_action_server_ = node->create_action_server<DockRobot>(
55 nullptr,
nullptr, std::chrono::milliseconds(500),
58 undocking_action_server_ = node->create_action_server<UndockRobot>(
61 nullptr,
nullptr, std::chrono::milliseconds(500),
65 controller_ = std::make_unique<Controller>(node, tf2_buffer_, params_->base_frame);
66 navigator_ = std::make_unique<Navigator>(node);
67 dock_db_ = std::make_unique<DockDatabase>(param_handler_->getMutex());
68 if (!dock_db_->initialize(node, tf2_buffer_)) {
70 return nav2::CallbackReturn::FAILURE;
73 return nav2::CallbackReturn::SUCCESS;
79 RCLCPP_INFO(get_logger(),
"Activating %s", get_name());
83 tf2_listener_ = nav2::create_transform_listener(*tf2_buffer_,
this,
true);
85 navigator_->activate();
86 vel_publisher_->on_activate();
87 docking_action_server_->activate();
88 undocking_action_server_->activate();
89 param_handler_->activate();
90 curr_dock_type_.clear();
95 return nav2::CallbackReturn::SUCCESS;
101 RCLCPP_INFO(get_logger(),
"Deactivating %s", get_name());
103 docking_action_server_->deactivate();
104 undocking_action_server_->deactivate();
105 dock_db_->deactivate();
106 navigator_->deactivate();
107 vel_publisher_->on_deactivate();
108 param_handler_->deactivate();
109 tf2_listener_.reset();
114 return nav2::CallbackReturn::SUCCESS;
120 RCLCPP_INFO(get_logger(),
"Cleaning up %s", get_name());
122 docking_action_server_.reset();
123 undocking_action_server_.reset();
126 curr_dock_type_.clear();
128 vel_publisher_.reset();
129 params_->dock_backwards.reset();
131 return nav2::CallbackReturn::SUCCESS;
137 RCLCPP_INFO(get_logger(),
"Shutting down %s", get_name());
138 return nav2::CallbackReturn::SUCCESS;
141 template<
typename ActionT>
143 typename std::shared_ptr<const typename ActionT::Goal> goal,
144 const typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server)
151 template<
typename ActionT>
153 typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server,
154 const std::string & name)
157 RCLCPP_WARN(get_logger(),
"Goal was cancelled. Cancelling %s action", name.c_str());
163 template<
typename ActionT>
165 typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server,
166 const std::string & name)
169 RCLCPP_WARN(get_logger(),
"Goal was preempted. Cancelling %s action", name.c_str());
177 std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());
178 action_start_time_ = this->now();
179 nav2::Rate loop_rate(
this, params_->controller_frequency);
181 auto goal = docking_action_server_->get_current_goal();
182 auto result = std::make_shared<DockRobot::Result>();
183 result->success =
false;
185 if (!docking_action_server_ || !docking_action_server_->is_server_active()) {
186 RCLCPP_DEBUG(get_logger(),
"Action server unavailable or inactive. Stopping.");
190 if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_,
"dock_robot")) {
191 docking_action_server_->terminate_all();
195 getPreemptedGoalIfRequested<DockRobot>(goal, docking_action_server_);
196 Dock * dock{
nullptr};
201 if (goal->use_dock_id) {
204 "Attempting to dock robot at %s.", goal->dock_id.c_str());
205 dock = dock_db_->findDock(goal->dock_id);
209 "Attempting to dock robot at position (%0.2f, %0.2f).",
210 goal->dock_pose.pose.position.x, goal->dock_pose.pose.position.y);
215 if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) {
217 get_logger(),
"Robot is already docked and/or charging (if applicable), no need to dock");
218 result->success =
true;
219 docking_action_server_->succeeded_current(result);
225 const auto initial_staging_pose = dock->getStagingPose();
227 if (!goal->navigate_to_staging_pose ||
228 utils::l2Norm(robot_pose.pose,
229 initial_staging_pose.pose) < params_->dock_prestaging_tolerance)
231 RCLCPP_INFO(get_logger(),
"Robot already within pre-staging pose tolerance for dock");
233 std::function<bool()> isPreempted = [
this]() {
234 return checkAndWarnIfCancelled<DockRobot>(docking_action_server_,
"dock_robot") ||
235 checkAndWarnIfPreempted<DockRobot>(docking_action_server_,
"dock_robot");
238 navigator_->goToPose(
239 initial_staging_pose,
240 rclcpp::Duration::from_seconds(goal->max_staging_time),
242 RCLCPP_INFO(get_logger(),
"Successful navigation to staging pose");
246 auto dock_pose = utils::getDockPoseStamped(dock, rclcpp::Time(0));
247 tf2_buffer_->transform(dock_pose, dock_pose, params_->fixed_frame);
251 RCLCPP_INFO(get_logger(),
"Successful initial dock detection");
254 bool dock_backward = params_->dock_backwards.has_value() ?
255 params_->dock_backwards.value() :
256 (dock->plugin->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
260 auto staging_pose = dock->getStagingPose();
261 if (dock->plugin->shouldRotateToDock()) {
262 staging_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
263 tf2::getYaw(staging_pose.pose.orientation) + M_PI);
267 while (rclcpp::ok()) {
270 if (dock->plugin->shouldRotateToDock()) {
277 get_logger(),
"Made contact with dock, waiting for charge to start (if applicable).");
280 if (dock->plugin->isCharger()) {
281 RCLCPP_INFO(get_logger(),
"Robot is charging!");
283 RCLCPP_INFO(get_logger(),
"Docking was successful!");
285 result->success =
true;
286 result->num_retries = num_retries_;
288 dock->plugin->stopDetectionProcess();
290 docking_action_server_->succeeded_current(result);
297 dock->plugin->stopDetectionProcess();
299 docking_action_server_->terminate_all(result);
302 if (++num_retries_ > params_->max_retries) {
303 RCLCPP_ERROR(get_logger(),
"Failed to dock, all retries have been used");
304 if (params_->max_retries > 0) {
307 }
catch (
const std::exception & ex) {
309 get_logger(),
"Failed to return to staging pose: %s", ex.what());
314 RCLCPP_WARN(get_logger(),
"Docking failed, will retry: %s", e.what());
321 dock->plugin->stopDetectionProcess();
323 docking_action_server_->terminate_all(result);
326 RCLCPP_INFO(get_logger(),
"Returned to staging pose, attempting docking again");
328 }
catch (
const tf2::TransformException & e) {
329 result->error_msg = std::string(
"Transform error: ") + e.what();
330 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
331 result->error_code = DockRobot::Result::UNKNOWN;
333 result->error_msg = e.what();
334 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
335 result->error_code = DockRobot::Result::DOCK_NOT_IN_DB;
337 result->error_msg = e.what();
338 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
339 result->error_code = DockRobot::Result::DOCK_NOT_VALID;
341 result->error_msg = e.what();
342 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
343 result->error_code = DockRobot::Result::FAILED_TO_STAGE;
345 result->error_msg = e.what();
346 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
347 result->error_code = DockRobot::Result::FAILED_TO_DETECT_DOCK;
349 result->error_msg = e.what();
350 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
351 result->error_code = DockRobot::Result::FAILED_TO_CONTROL;
353 result->error_msg = e.what();
354 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
355 result->error_code = DockRobot::Result::FAILED_TO_CHARGE;
357 result->error_msg = e.what();
358 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
359 result->error_code = DockRobot::Result::UNKNOWN;
360 }
catch (std::exception & e) {
361 result->error_code = DockRobot::Result::UNKNOWN;
362 result->error_msg = e.what();
363 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
366 result->num_retries = num_retries_;
369 dock->plugin->stopDetectionProcess();
373 docking_action_server_->terminate_current(result);
378 if (dock && successful) {
379 curr_dock_type_ = dock->type;
382 if (!use_dock_id && dock) {
390 auto plugin = dock_db_->findDockPlugin(goal->dock_type);
393 "Dock type '" + goal->dock_type +
"' has no valid plugin!");
396 auto dock =
new Dock();
397 dock->frame = goal->dock_pose.header.frame_id;
398 dock->pose = goal->dock_pose.pose;
399 dock->type = goal->dock_type;
400 dock->plugin = plugin;
408 if (!dock->plugin->startDetectionProcess()) {
412 nav2::Rate loop_rate(
this, params_->controller_frequency);
413 auto start = this->now();
414 auto timeout = rclcpp::Duration::from_seconds(params_->initial_perception_timeout);
415 while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
416 if (this->now() - start > timeout) {
418 "Failed initial dock detection: Timeout exceeded");
421 if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_,
"dock_robot") ||
422 checkAndWarnIfPreempted<DockRobot>(docking_action_server_,
"dock_robot"))
433 const double dt = 1.0 / params_->controller_frequency;
434 auto target_pose = dock_pose;
435 target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
436 tf2::getYaw(target_pose.pose.orientation) + M_PI);
438 nav2::Rate loop_rate(
this, params_->controller_frequency);
439 auto start = this->now();
440 auto timeout = rclcpp::Duration::from_seconds(params_->rotate_to_dock_timeout);
442 while (rclcpp::ok()) {
444 auto angular_distance_to_heading = angles::shortest_angular_distance(
445 tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(target_pose.pose.orientation));
446 if (fabs(angular_distance_to_heading) < params_->rotation_angular_tolerance) {
450 auto current_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
451 current_vel->twist.angular.z = odom_sub_->getRawTwist().angular.z;
453 auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
454 command->header = robot_pose.header;
455 command->twist = controller_->computeRotateToHeadingCommand(
456 angular_distance_to_heading, current_vel->twist, dt);
458 vel_publisher_->publish(std::move(command));
460 if (this->now() - start > timeout) {
469 Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose,
bool backward)
471 nav2::Rate loop_rate(
this, params_->controller_frequency);
472 auto start = this->now();
473 auto timeout = rclcpp::Duration::from_seconds(params_->dock_approach_timeout);
475 while (rclcpp::ok()) {
479 if (dock->plugin->isDocked() || (dock->plugin->isCharger() && dock->plugin->isCharging())) {
484 if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_,
"dock_robot") ||
485 checkAndWarnIfPreempted<DockRobot>(docking_action_server_,
"dock_robot"))
491 if (!dock->plugin->getRefinedPose(dock_pose, dock->id) && !dock->plugin->shouldRotateToDock()) {
496 geometry_msgs::msg::PoseStamped target_pose = utils::transformPoseToFrame(
497 *tf2_buffer_, dock_pose, params_->fixed_frame, now(), params_->transform_staleness_threshold);
503 const double backward_projection = 0.25;
504 const double yaw = tf2::getYaw(target_pose.pose.orientation);
505 target_pose.pose.position.x += cos(yaw) * backward_projection;
506 target_pose.pose.position.y += sin(yaw) * backward_projection;
508 const auto base_to_fixed_transform = nav2_util::poseToTransformStamped(
509 robot_pose, params_->base_frame);
512 tf2::doTransform(target_pose, target_pose, nav2_util::invertTransform(base_to_fixed_transform));
517 target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
518 tf2::getYaw(target_pose.pose.orientation) + M_PI);
522 auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
523 command->header.stamp = now();
524 if (!controller_->computeVelocityCommand(
525 target_pose.pose, command->twist, base_to_fixed_transform,
true, backward))
529 vel_publisher_->publish(std::move(command));
531 if (this->now() - start > timeout) {
533 "Timed out approaching dock; dock nor charging (if applicable) detected");
544 if (!dock->plugin->isCharger()) {
548 nav2::Rate loop_rate(
this, params_->controller_frequency);
549 auto start = this->now();
550 auto timeout = rclcpp::Duration::from_seconds(params_->wait_charge_timeout);
551 while (rclcpp::ok()) {
554 if (dock->plugin->isCharging()) {
558 if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_,
"dock_robot") ||
559 checkAndWarnIfPreempted<DockRobot>(docking_action_server_,
"dock_robot"))
564 if (this->now() - start > timeout) {
574 const geometry_msgs::msg::PoseStamped & staging_pose,
bool backward)
576 const auto fixed_staging_pose = utils::transformPoseToFrame(
577 *tf2_buffer_, staging_pose, params_->fixed_frame, now(),
578 params_->transform_staleness_threshold);
580 nav2::Rate loop_rate(
this, params_->controller_frequency);
581 auto start = this->now();
582 auto timeout = rclcpp::Duration::from_seconds(params_->dock_approach_timeout);
583 while (rclcpp::ok()) {
587 if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_,
"dock_robot") ||
588 checkAndWarnIfPreempted<DockRobot>(docking_action_server_,
"dock_robot"))
594 auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
595 command->header.stamp = now();
597 command->twist, fixed_staging_pose, params_->undock_linear_tolerance,
598 params_->undock_angular_tolerance,
false,
603 vel_publisher_->publish(std::move(command));
605 if (this->now() - start > timeout) {
615 geometry_msgs::msg::Twist & cmd,
const geometry_msgs::msg::PoseStamped & pose,
616 double linear_tolerance,
double angular_tolerance,
bool is_docking,
bool backward)
622 geometry_msgs::msg::PoseStamped fixed_pose = utils::transformPoseToFrame(
623 *tf2_buffer_, pose, params_->fixed_frame, now(), params_->transform_staleness_threshold);
627 const auto base_to_fixed_transform = nav2_util::poseToTransformStamped(robot_pose,
628 params_->base_frame);
629 const double dist = std::hypot(
630 robot_pose.pose.position.x - fixed_pose.pose.position.x,
631 robot_pose.pose.position.y - fixed_pose.pose.position.y);
632 const double yaw = angles::shortest_angular_distance(
633 tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(fixed_pose.pose.orientation));
634 if (dist < linear_tolerance && abs(yaw) < angular_tolerance) {
639 geometry_msgs::msg::PoseStamped target_pose = fixed_pose;
640 tf2::doTransform(target_pose, target_pose, nav2_util::invertTransform(base_to_fixed_transform));
643 if (!controller_->computeVelocityCommand(
644 target_pose.pose, cmd, base_to_fixed_transform, is_docking, backward))
655 std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());
656 action_start_time_ = this->now();
657 nav2::Rate loop_rate(
this, params_->controller_frequency);
659 auto goal = undocking_action_server_->get_current_goal();
660 auto result = std::make_shared<UndockRobot::Result>();
661 result->success =
false;
663 if (!undocking_action_server_ || !undocking_action_server_->is_server_active()) {
664 RCLCPP_DEBUG(get_logger(),
"Action server unavailable or inactive. Stopping.");
668 if (checkAndWarnIfCancelled<UndockRobot>(undocking_action_server_,
"undock_robot")) {
669 undocking_action_server_->terminate_all(result);
673 getPreemptedGoalIfRequested<UndockRobot>(goal, undocking_action_server_);
674 auto max_duration = rclcpp::Duration::from_seconds(goal->max_undocking_time);
678 std::string dock_type = curr_dock_type_;
679 if (!goal->dock_type.empty()) {
680 dock_type = goal->dock_type;
683 ChargingDock::Ptr dock = dock_db_->findDockPlugin(dock_type);
689 "Attempting to undock robot of dock type %s.", dock->getName().c_str());
692 if (dock->isCharger() && (!dock->isDocked() && !dock->isCharging())) {
693 RCLCPP_INFO(get_logger(),
"Robot is not in the dock, no need to undock");
697 bool dock_backward = params_->dock_backwards.has_value() ?
698 params_->dock_backwards.value() :
699 (dock->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
706 dock_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
707 tf2::getYaw(dock_pose.pose.orientation) + M_PI);
711 geometry_msgs::msg::PoseStamped staging_pose =
712 dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id);
716 if (dock->shouldRotateToDock()) {
717 staging_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
718 tf2::getYaw(staging_pose.pose.orientation) + M_PI);
722 rclcpp::Time loop_start = this->now();
723 while (rclcpp::ok()) {
725 auto timeout = rclcpp::Duration::from_seconds(goal->max_undocking_time);
726 if (this->now() - loop_start > timeout) {
731 if (checkAndWarnIfCancelled<UndockRobot>(undocking_action_server_,
"undock_robot") ||
732 checkAndWarnIfPreempted<UndockRobot>(undocking_action_server_,
"undock_robot"))
735 undocking_action_server_->terminate_all(result);
740 if (dock->isCharger() && !dock->disableCharging()) {
746 auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
747 command->header.stamp = now();
750 command->twist, staging_pose, params_->undock_linear_tolerance,
751 params_->undock_angular_tolerance,
false,
755 if (dock->shouldRotateToDock()) {
760 RCLCPP_INFO(get_logger(),
"Robot has reached staging pose");
761 vel_publisher_->publish(std::move(command));
762 if (!dock->isCharger() || dock->hasStoppedCharging()) {
763 RCLCPP_INFO(get_logger(),
"Robot has undocked!");
764 result->success =
true;
765 curr_dock_type_.clear();
767 undocking_action_server_->succeeded_current(result);
775 vel_publisher_->publish(std::move(command));
778 }
catch (
const tf2::TransformException & e) {
779 result->error_msg = std::string(
"Transform error: ") + e.what();
780 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
781 result->error_code = DockRobot::Result::UNKNOWN;
783 result->error_msg = e.what();
784 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
785 result->error_code = DockRobot::Result::DOCK_NOT_VALID;
787 result->error_msg = e.what();
788 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
789 result->error_code = DockRobot::Result::FAILED_TO_CONTROL;
791 result->error_msg = e.what();
792 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
793 result->error_code = DockRobot::Result::UNKNOWN;
794 }
catch (std::exception & e) {
795 result->error_msg = std::string(
"Internal error: ") + e.what();
796 RCLCPP_ERROR(get_logger(),
"%s", result->error_msg.c_str());
797 result->error_code = DockRobot::Result::UNKNOWN;
801 undocking_action_server_->terminate_current(result);
806 geometry_msgs::msg::PoseStamped robot_pose;
807 if (!nav2_util::getFreshPose(
808 *tf2_buffer_, frame, params_->base_frame, now(),
809 params_->transform_staleness_threshold, robot_pose))
812 "Transform error: failed to get a fresh robot pose in frame " + frame);
819 auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
820 cmd_vel->header.stamp = now();
821 vel_publisher_->publish(std::move(cmd_vel));
826 auto feedback = std::make_shared<DockRobot::Feedback>();
827 feedback->state = state;
828 feedback->docking_time = this->now() - action_start_time_;
829 feedback->num_retries = num_retries_;
830 docking_action_server_->publish_feedback(feedback);
834 #include "rclcpp_components/register_node_macro.hpp"
void destroyBond()
Destroy bond connection to lifecycle manager.
nav2::LifecycleNode::SharedPtr shared_from_this()
Get a shared pointer of this.
void createBond()
Create bond connection to lifecycle manager.
A sim-time-aware rate for Nav2 loops.
bool is_cancel_requested() const
Whether or not a cancel command has come in.
bool is_preempt_requested() const
Whether the action server has been asked to be preempted with a new goal.
const std::shared_ptr< const typename ActionT::Goal > accept_pending_goal()
Accept pending goals.
An action server which implements charger docking node for AMRs.
virtual geometry_msgs::msg::PoseStamped getRobotPoseInFrame(const std::string &frame)
Get the robot pose (aka base_frame pose) in another frame.
bool resetApproach(const geometry_msgs::msg::PoseStamped &staging_pose, bool backward)
Reset the robot for another approach by controlling back to staging pose.
void dockRobot()
Main action callback method to complete docking request.
nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override
Deactivate member variables.
nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override
Reset member variables.
bool approachDock(Dock *dock, geometry_msgs::msg::PoseStamped &dock_pose, bool backward)
Use control law and dock perception to approach the charge dock.
void doInitialPerception(Dock *dock, geometry_msgs::msg::PoseStamped &dock_pose)
Do initial perception, up to a timeout.
void publishDockingFeedback(uint16_t state)
Publish feedback from a docking action.
bool checkAndWarnIfCancelled(typename nav2::SimpleActionServer< ActionT >::SharedPtr &action_server, const std::string &name)
Checks and logs warning if action canceled.
nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override
Called when in shutdown state.
bool checkAndWarnIfPreempted(typename nav2::SimpleActionServer< ActionT >::SharedPtr &action_server, const std::string &name)
Checks and logs warning if action preempted.
bool getCommandToPose(geometry_msgs::msg::Twist &cmd, const geometry_msgs::msg::PoseStamped &pose, double linear_tolerance, double angular_tolerance, bool is_docking, bool backward)
Run a single iteration of the control loop to approach a pose.
bool waitForCharge(Dock *dock)
Wait for charging to begin.
Dock * generateGoalDock(std::shared_ptr< const DockRobot::Goal > goal)
Generate a dock from action goal.
nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override
Activate member variables.
void getPreemptedGoalIfRequested(typename std::shared_ptr< const typename ActionT::Goal > goal, const typename nav2::SimpleActionServer< ActionT >::SharedPtr &action_server)
Gets a preempted goal if immediately requested.
nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override
Configure member variables.
void publishZeroVelocity()
Publish zero velocity at terminal condition.
void rotateToDock(const geometry_msgs::msg::PoseStamped &dock_pose)
Perform a pure rotation to dock orientation.
void undockRobot()
Main action callback method to complete undocking request.
void stashDockData(bool use_dock_id, Dock *dock, bool successful)
Called at the conclusion of docking actions. Saves relevant docking data for later undocking action.
Dock was not found in the provided dock database.
Dock plugin provided in the database or action was invalid.
Abstract docking exception.
A required transform was unavailable or stale.
Failed to start charging.
Failed to control into or out of the dock.
Failed to detect the charging dock.
Failed to navigate to the staging pose.