Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
docking_server.cpp
1 // Copyright (c) 2024 Open Navigation LLC
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
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"
21 
22 using namespace std::chrono_literals;
23 using rcl_interfaces::msg::ParameterType;
24 using std::placeholders::_1;
25 
26 namespace opennav_docking
27 {
28 
29 DockingServer::DockingServer(const rclcpp::NodeOptions & options)
30 : nav2::LifecycleNode("docking_server", "", options)
31 {
32  RCLCPP_INFO(get_logger(), "Creating %s", get_name());
33 }
34 
35 nav2::CallbackReturn
36 DockingServer::on_configure(const rclcpp_lifecycle::State & state)
37 {
38  RCLCPP_INFO(get_logger(), "Configuring %s", get_name());
39  auto node = shared_from_this();
40  param_handler_ = std::make_unique<ParameterHandler>(
41  node, get_logger());
42  params_ = param_handler_->getParams();
43 
44  vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel");
45  tf2_buffer_ = nav2::create_transform_buffer(node);
46 
47  // Create odom subscriber for backward blind docking
48  odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, params_->odom_duration,
49  params_->odom_topic);
50 
51  // Create the action servers for dock / undock
52  docking_action_server_ = node->create_action_server<DockRobot>(
53  "dock_robot",
54  std::bind(&DockingServer::dockRobot, this),
55  nullptr, nullptr, std::chrono::milliseconds(500),
56  true);
57 
58  undocking_action_server_ = node->create_action_server<UndockRobot>(
59  "undock_robot",
60  std::bind(&DockingServer::undockRobot, this),
61  nullptr, nullptr, std::chrono::milliseconds(500),
62  true);
63 
64  // Create composed utilities
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_)) {
69  on_cleanup(state);
70  return nav2::CallbackReturn::FAILURE;
71  }
72 
73  return nav2::CallbackReturn::SUCCESS;
74 }
75 
76 nav2::CallbackReturn
77 DockingServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
78 {
79  RCLCPP_INFO(get_logger(), "Activating %s", get_name());
80 
81  auto node = shared_from_this();
82 
83  tf2_listener_ = nav2::create_transform_listener(*tf2_buffer_, this, true);
84  dock_db_->activate();
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();
91 
92  // Create bond connection
93  createBond();
94 
95  return nav2::CallbackReturn::SUCCESS;
96 }
97 
98 nav2::CallbackReturn
99 DockingServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
100 {
101  RCLCPP_INFO(get_logger(), "Deactivating %s", get_name());
102 
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();
110 
111  // Destroy bond connection
112  destroyBond();
113 
114  return nav2::CallbackReturn::SUCCESS;
115 }
116 
117 nav2::CallbackReturn
118 DockingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
119 {
120  RCLCPP_INFO(get_logger(), "Cleaning up %s", get_name());
121  tf2_buffer_.reset();
122  docking_action_server_.reset();
123  undocking_action_server_.reset();
124  dock_db_.reset();
125  navigator_.reset();
126  curr_dock_type_.clear();
127  controller_.reset();
128  vel_publisher_.reset();
129  params_->dock_backwards.reset();
130  odom_sub_.reset();
131  return nav2::CallbackReturn::SUCCESS;
132 }
133 
134 nav2::CallbackReturn
135 DockingServer::on_shutdown(const rclcpp_lifecycle::State &)
136 {
137  RCLCPP_INFO(get_logger(), "Shutting down %s", get_name());
138  return nav2::CallbackReturn::SUCCESS;
139 }
140 
141 template<typename ActionT>
143  typename std::shared_ptr<const typename ActionT::Goal> goal,
144  const typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server)
145 {
146  if (action_server->is_preempt_requested()) {
147  goal = action_server->accept_pending_goal();
148  }
149 }
150 
151 template<typename ActionT>
153  typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server,
154  const std::string & name)
155 {
156  if (action_server->is_cancel_requested()) {
157  RCLCPP_WARN(get_logger(), "Goal was cancelled. Cancelling %s action", name.c_str());
158  return true;
159  }
160  return false;
161 }
162 
163 template<typename ActionT>
165  typename nav2::SimpleActionServer<ActionT>::SharedPtr & action_server,
166  const std::string & name)
167 {
168  if (action_server->is_preempt_requested()) {
169  RCLCPP_WARN(get_logger(), "Goal was preempted. Cancelling %s action", name.c_str());
170  return true;
171  }
172  return false;
173 }
174 
176 {
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);
180 
181  auto goal = docking_action_server_->get_current_goal();
182  auto result = std::make_shared<DockRobot::Result>();
183  result->success = false;
184 
185  if (!docking_action_server_ || !docking_action_server_->is_server_active()) {
186  RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
187  return;
188  }
189 
190  if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_, "dock_robot")) {
191  docking_action_server_->terminate_all();
192  return;
193  }
194 
195  getPreemptedGoalIfRequested<DockRobot>(goal, docking_action_server_);
196  Dock * dock{nullptr};
197  num_retries_ = 0;
198 
199  try {
200  // Get dock (instance and plugin information) from request
201  if (goal->use_dock_id) {
202  RCLCPP_INFO(
203  get_logger(),
204  "Attempting to dock robot at %s.", goal->dock_id.c_str());
205  dock = dock_db_->findDock(goal->dock_id);
206  } else {
207  RCLCPP_INFO(
208  get_logger(),
209  "Attempting to dock robot at position (%0.2f, %0.2f).",
210  goal->dock_pose.pose.position.x, goal->dock_pose.pose.position.y);
211  dock = generateGoalDock(goal);
212  }
213 
214  // Check if robot is docked or charging before proceeding, only applicable to charging docks
215  if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) {
216  RCLCPP_INFO(
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);
220  return;
221  }
222 
223  // Send robot to its staging pose
224  publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE);
225  const auto initial_staging_pose = dock->getStagingPose();
226  const auto robot_pose = getRobotPoseInFrame(initial_staging_pose.header.frame_id);
227  if (!goal->navigate_to_staging_pose ||
228  utils::l2Norm(robot_pose.pose,
229  initial_staging_pose.pose) < params_->dock_prestaging_tolerance)
230  {
231  RCLCPP_INFO(get_logger(), "Robot already within pre-staging pose tolerance for dock");
232  } else {
233  std::function<bool()> isPreempted = [this]() {
234  return checkAndWarnIfCancelled<DockRobot>(docking_action_server_, "dock_robot") ||
235  checkAndWarnIfPreempted<DockRobot>(docking_action_server_, "dock_robot");
236  };
237 
238  navigator_->goToPose(
239  initial_staging_pose,
240  rclcpp::Duration::from_seconds(goal->max_staging_time),
241  isPreempted);
242  RCLCPP_INFO(get_logger(), "Successful navigation to staging pose");
243  }
244 
245  // Construct initial estimate of where the dock is located in fixed_frame
246  auto dock_pose = utils::getDockPoseStamped(dock, rclcpp::Time(0));
247  tf2_buffer_->transform(dock_pose, dock_pose, params_->fixed_frame);
248 
249  // Get initial detection of dock before proceeding to move
250  doInitialPerception(dock, dock_pose);
251  RCLCPP_INFO(get_logger(), "Successful initial dock detection");
252 
253  // Get the direction of the movement
254  bool dock_backward = params_->dock_backwards.has_value() ?
255  params_->dock_backwards.value() :
256  (dock->plugin->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
257 
258  // If we performed a rotation before docking backward, we must rotate the staging pose
259  // to match the robot orientation
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);
264  }
265 
266  // Docking control loop: while not docked, run controller
267  while (rclcpp::ok()) {
268  try {
269  // Perform a 180º to face away from the dock if needed
270  if (dock->plugin->shouldRotateToDock()) {
271  rotateToDock(dock_pose);
272  }
273  // Approach the dock using control law
274  if (approachDock(dock, dock_pose, dock_backward)) {
275  // We are docked, wait for charging to begin
276  RCLCPP_INFO(
277  get_logger(), "Made contact with dock, waiting for charge to start (if applicable).");
279  if (waitForCharge(dock)) {
280  if (dock->plugin->isCharger()) {
281  RCLCPP_INFO(get_logger(), "Robot is charging!");
282  } else {
283  RCLCPP_INFO(get_logger(), "Docking was successful!");
284  }
285  result->success = true;
286  result->num_retries = num_retries_;
288  dock->plugin->stopDetectionProcess();
289  stashDockData(goal->use_dock_id, dock, true);
290  docking_action_server_->succeeded_current(result);
291  return;
292  }
293  }
294 
295  // Cancelled, preempted, or shutting down (recoverable errors throw DockingException)
297  dock->plugin->stopDetectionProcess();
298  stashDockData(goal->use_dock_id, dock, false);
299  docking_action_server_->terminate_all(result);
300  return;
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) {
305  try { // swallow new exceptions, so as to report original failure
306  resetApproach(staging_pose, dock_backward);
307  } catch (const std::exception & ex) {
308  RCLCPP_ERROR(
309  get_logger(), "Failed to return to staging pose: %s", ex.what());
310  }
311  }
312  throw;
313  }
314  RCLCPP_WARN(get_logger(), "Docking failed, will retry: %s", e.what());
315  }
316 
317  // Reset to staging pose to try again
318  if (!resetApproach(staging_pose, dock_backward)) {
319  // Cancelled, preempted, or shutting down
321  dock->plugin->stopDetectionProcess();
322  stashDockData(goal->use_dock_id, dock, false);
323  docking_action_server_->terminate_all(result);
324  return;
325  }
326  RCLCPP_INFO(get_logger(), "Returned to staging pose, attempting docking again");
327  }
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;
332  } catch (opennav_docking_core::DockNotInDB & e) {
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;
336  } catch (opennav_docking_core::DockNotValid & e) {
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());
364  }
365 
366  result->num_retries = num_retries_;
368  if (dock) {
369  dock->plugin->stopDetectionProcess();
370  }
371  // Store dock state for later undocking and delete temp dock, if applicable
372  stashDockData(goal->use_dock_id, dock, false);
373  docking_action_server_->terminate_current(result);
374 }
375 
376 void DockingServer::stashDockData(bool use_dock_id, Dock * dock, bool successful)
377 {
378  if (dock && successful) {
379  curr_dock_type_ = dock->type;
380  }
381 
382  if (!use_dock_id && dock) {
383  delete dock;
384  dock = nullptr;
385  }
386 }
387 
388 Dock * DockingServer::generateGoalDock(std::shared_ptr<const DockRobot::Goal> goal)
389 {
390  auto plugin = dock_db_->findDockPlugin(goal->dock_type);
391  if (!plugin) {
393  "Dock type '" + goal->dock_type + "' has no valid plugin!");
394  }
395 
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;
401  return dock;
402 }
403 
404 void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose)
405 {
406  publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION);
407 
408  if (!dock->plugin->startDetectionProcess()) {
409  throw opennav_docking_core::FailedToDetectDock("Failed to start the detection process.");
410  }
411 
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");
419  }
420 
421  if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_, "dock_robot") ||
422  checkAndWarnIfPreempted<DockRobot>(docking_action_server_, "dock_robot"))
423  {
424  return;
425  }
426 
427  loop_rate.sleep();
428  }
429 }
430 
431 void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_pose)
432 {
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);
437 
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);
441 
442  while (rclcpp::ok()) {
443  auto robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id);
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) {
447  break;
448  }
449 
450  auto current_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
451  current_vel->twist.angular.z = odom_sub_->getRawTwist().angular.z;
452 
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);
457 
458  vel_publisher_->publish(std::move(command));
459 
460  if (this->now() - start > timeout) {
461  throw opennav_docking_core::FailedToControl("Timed out rotating to dock");
462  }
463 
464  loop_rate.sleep();
465  }
466 }
467 
469  Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward)
470 {
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);
474 
475  while (rclcpp::ok()) {
476  publishDockingFeedback(DockRobot::Feedback::CONTROLLING);
477 
478  // Stop and report success if connected to dock
479  if (dock->plugin->isDocked() || (dock->plugin->isCharger() && dock->plugin->isCharging())) {
480  return true;
481  }
482 
483  // Stop if cancelled/preempted
484  if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_, "dock_robot") ||
485  checkAndWarnIfPreempted<DockRobot>(docking_action_server_, "dock_robot"))
486  {
487  return false;
488  }
489 
490  // Update perception
491  if (!dock->plugin->getRefinedPose(dock_pose, dock->id) && !dock->plugin->shouldRotateToDock()) {
492  throw opennav_docking_core::FailedToDetectDock("Failed dock detection");
493  }
494 
495  // Transform target_pose into base_link frame
496  geometry_msgs::msg::PoseStamped target_pose = utils::transformPoseToFrame(
497  *tf2_buffer_, dock_pose, params_->fixed_frame, now(), params_->transform_staleness_threshold);
498 
499  // The control law can get jittery when close to the end when atan2's can explode.
500  // Thus, we backward project the controller's target pose a little bit after the
501  // dock so that the robot never gets to the end of the spiral before its in contact
502  // with the dock to stop the docking procedure.
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;
507  const auto robot_pose = getRobotPoseInFrame(params_->fixed_frame);
508  const auto base_to_fixed_transform = nav2_util::poseToTransformStamped(
509  robot_pose, params_->base_frame);
510  // Transforming the target pose from the fixed frame into the robot's base frame,
511  // using the inverse of the robot pose transform that is also passed to the controller.
512  tf2::doTransform(target_pose, target_pose, nav2_util::invertTransform(base_to_fixed_transform));
513 
514  // Make sure that the target pose is pointing at the robot when moving backwards
515  // This is to ensure that the robot doesn't try to dock from the wrong side
516  if (backward) {
517  target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
518  tf2::getYaw(target_pose.pose.orientation) + M_PI);
519  }
520 
521  // Compute and publish controls
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))
526  {
527  throw opennav_docking_core::FailedToControl("Failed to get control");
528  }
529  vel_publisher_->publish(std::move(command));
530 
531  if (this->now() - start > timeout) {
533  "Timed out approaching dock; dock nor charging (if applicable) detected");
534  }
535 
536  loop_rate.sleep();
537  }
538  return false;
539 }
540 
542 {
543  // This is a non-charger docking request
544  if (!dock->plugin->isCharger()) {
545  return true;
546  }
547 
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()) {
552  publishDockingFeedback(DockRobot::Feedback::WAIT_FOR_CHARGE);
553 
554  if (dock->plugin->isCharging()) {
555  return true;
556  }
557 
558  if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_, "dock_robot") ||
559  checkAndWarnIfPreempted<DockRobot>(docking_action_server_, "dock_robot"))
560  {
561  return false;
562  }
563 
564  if (this->now() - start > timeout) {
565  throw opennav_docking_core::FailedToCharge("Timed out waiting for charge to start");
566  }
567 
568  loop_rate.sleep();
569  }
570  return false;
571 }
572 
574  const geometry_msgs::msg::PoseStamped & staging_pose, bool backward)
575 {
576  const auto fixed_staging_pose = utils::transformPoseToFrame(
577  *tf2_buffer_, staging_pose, params_->fixed_frame, now(),
578  params_->transform_staleness_threshold);
579 
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()) {
584  publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION);
585 
586  // Stop if cancelled/preempted
587  if (checkAndWarnIfCancelled<DockRobot>(docking_action_server_, "dock_robot") ||
588  checkAndWarnIfPreempted<DockRobot>(docking_action_server_, "dock_robot"))
589  {
590  return false;
591  }
592 
593  // Compute and publish command
594  auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
595  command->header.stamp = now();
596  if (getCommandToPose(
597  command->twist, fixed_staging_pose, params_->undock_linear_tolerance,
598  params_->undock_angular_tolerance, false,
599  !backward))
600  {
601  return true;
602  }
603  vel_publisher_->publish(std::move(command));
604 
605  if (this->now() - start > timeout) {
606  throw opennav_docking_core::FailedToControl("Timed out resetting dock approach");
607  }
608 
609  loop_rate.sleep();
610  }
611  return false;
612 }
613 
615  geometry_msgs::msg::Twist & cmd, const geometry_msgs::msg::PoseStamped & pose,
616  double linear_tolerance, double angular_tolerance, bool is_docking, bool backward)
617 {
618  // Reset command to zero velocity
619  cmd.linear.x = 0;
620  cmd.angular.z = 0;
621 
622  geometry_msgs::msg::PoseStamped fixed_pose = utils::transformPoseToFrame(
623  *tf2_buffer_, pose, params_->fixed_frame, now(), params_->transform_staleness_threshold);
624 
625  // Determine if we have reached pose yet & stop
626  const auto robot_pose = getRobotPoseInFrame(params_->fixed_frame);
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) {
635  return true;
636  }
637 
638  // Transform target_pose into base_link frame
639  geometry_msgs::msg::PoseStamped target_pose = fixed_pose;
640  tf2::doTransform(target_pose, target_pose, nav2_util::invertTransform(base_to_fixed_transform));
641 
642  // Compute velocity command
643  if (!controller_->computeVelocityCommand(
644  target_pose.pose, cmd, base_to_fixed_transform, is_docking, backward))
645  {
646  throw opennav_docking_core::FailedToControl("Failed to get control");
647  }
648 
649  // Command is valid, but target is not reached
650  return false;
651 }
652 
654 {
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);
658 
659  auto goal = undocking_action_server_->get_current_goal();
660  auto result = std::make_shared<UndockRobot::Result>();
661  result->success = false;
662 
663  if (!undocking_action_server_ || !undocking_action_server_->is_server_active()) {
664  RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
665  return;
666  }
667 
668  if (checkAndWarnIfCancelled<UndockRobot>(undocking_action_server_, "undock_robot")) {
669  undocking_action_server_->terminate_all(result);
670  return;
671  }
672 
673  getPreemptedGoalIfRequested<UndockRobot>(goal, undocking_action_server_);
674  auto max_duration = rclcpp::Duration::from_seconds(goal->max_undocking_time);
675 
676  try {
677  // Get dock plugin information from request or docked state, reset state.
678  std::string dock_type = curr_dock_type_;
679  if (!goal->dock_type.empty()) {
680  dock_type = goal->dock_type;
681  }
682 
683  ChargingDock::Ptr dock = dock_db_->findDockPlugin(dock_type);
684  if (!dock) {
685  throw opennav_docking_core::DockNotValid("No dock information to undock from!");
686  }
687  RCLCPP_INFO(
688  get_logger(),
689  "Attempting to undock robot of dock type %s.", dock->getName().c_str());
690 
691  // Check if the robot is docked before proceeding
692  if (dock->isCharger() && (!dock->isDocked() && !dock->isCharging())) {
693  RCLCPP_INFO(get_logger(), "Robot is not in the dock, no need to undock");
694  return;
695  }
696 
697  bool dock_backward = params_->dock_backwards.has_value() ?
698  params_->dock_backwards.value() :
699  (dock->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
700 
701  // Get "dock pose" by finding the robot pose
702  geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(params_->fixed_frame);
703 
704  // Make sure that the staging pose is pointing in the same direction when moving backwards
705  if (dock_backward) {
706  dock_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
707  tf2::getYaw(dock_pose.pose.orientation) + M_PI);
708  }
709 
710  // Get staging pose (in fixed frame)
711  geometry_msgs::msg::PoseStamped staging_pose =
712  dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id);
713 
714  // If we performed a rotation before docking backward, we must rotate the staging pose
715  // to match the robot orientation
716  if (dock->shouldRotateToDock()) {
717  staging_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
718  tf2::getYaw(staging_pose.pose.orientation) + M_PI);
719  }
720 
721  // Control robot to staging pose
722  rclcpp::Time loop_start = this->now();
723  while (rclcpp::ok()) {
724  // Stop if we exceed max duration
725  auto timeout = rclcpp::Duration::from_seconds(goal->max_undocking_time);
726  if (this->now() - loop_start > timeout) {
727  throw opennav_docking_core::FailedToControl("Undocking timed out");
728  }
729 
730  // Stop if cancelled/preempted
731  if (checkAndWarnIfCancelled<UndockRobot>(undocking_action_server_, "undock_robot") ||
732  checkAndWarnIfPreempted<UndockRobot>(undocking_action_server_, "undock_robot"))
733  {
735  undocking_action_server_->terminate_all(result);
736  return;
737  }
738 
739  // Don't control the robot until charging is disabled
740  if (dock->isCharger() && !dock->disableCharging()) {
741  loop_rate.sleep();
742  continue;
743  }
744 
745  // Get command to approach staging pose
746  auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
747  command->header.stamp = now();
748 
749  if (getCommandToPose(
750  command->twist, staging_pose, params_->undock_linear_tolerance,
751  params_->undock_angular_tolerance, false,
752  !dock_backward))
753  {
754  // Perform a 180º to the original staging pose
755  if (dock->shouldRotateToDock()) {
756  rotateToDock(staging_pose);
757  }
758 
759  // Have reached staging_pose
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);
768  return;
769  }
770  // Haven't stopped charging?
771  throw opennav_docking_core::FailedToControl("Failed to control off dock");
772  }
773 
774  // Publish command and sleep
775  vel_publisher_->publish(std::move(command));
776  loop_rate.sleep();
777  }
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;
782  } catch (opennav_docking_core::DockNotValid & e) {
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;
798  }
799 
801  undocking_action_server_->terminate_current(result);
802 }
803 
804 geometry_msgs::msg::PoseStamped DockingServer::getRobotPoseInFrame(const std::string & frame)
805 {
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))
810  {
812  "Transform error: failed to get a fresh robot pose in frame " + frame);
813  }
814  return robot_pose;
815 }
816 
818 {
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));
822 }
823 
825 {
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);
831 }
832 } // namespace opennav_docking
833 
834 #include "rclcpp_components/register_node_macro.hpp"
835 
836 // Register the component with class_loader.
837 // This acts as a sort of entry point, allowing the component to be discoverable when its library
838 // is being loaded into a running process.
839 RCLCPP_COMPONENTS_REGISTER_NODE(opennav_docking::DockingServer)
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.
Definition: rate.hpp:61
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.
A required transform was unavailable or stale.
Failed to control into or out of the dock.
Failed to detect the charging dock.
Failed to navigate to the staging pose.
Definition: types.hpp:33