Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
simple_charging_dock.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 <cmath>
16 #include <chrono>
17 
18 #include "nav2_ros_common/node_utils.hpp"
19 #include "nav2_util/robot_utils.hpp"
20 #include "opennav_docking/simple_charging_dock.hpp"
21 #include "opennav_docking/utils.hpp"
22 #include "nav2_ros_common/tf2_factories.hpp"
23 
24 using namespace std::chrono_literals;
25 
26 namespace opennav_docking
27 {
28 
29 void SimpleChargingDock::configure(
30  const nav2::LifecycleNode::WeakPtr & parent,
31  const std::string & name, nav2::TransformBuffer::SharedPtr tf)
32 {
33  name_ = name;
34  tf2_buffer_ = tf;
35  is_charging_ = false;
36  node_ = parent.lock();
37  if (!node_) {
38  throw std::runtime_error{"Failed to lock node"};
39  }
40 
41  // Optionally use battery info to check when charging, else say charging if docked
42  use_battery_status_ = node_->declare_or_get_parameter(
43  name + ".use_battery_status", true);
44 
45  // Parameters for optional detector control
46  detector_service_name_ = node_->declare_or_get_parameter(
47  name + ".detector_service_name", std::string(""));
48  detector_service_timeout_ = node_->declare_or_get_parameter(
49  name + ".detector_service_timeout", 5.0);
50  subscribe_toggle_ = node_->declare_or_get_parameter(
51  name + ".subscribe_toggle", false);
52 
53  // Parameters for optional external detection of dock pose
54  use_external_detection_pose_ = node_->declare_or_get_parameter(
55  name + ".use_external_detection_pose", false);
56  external_detection_timeout_ = node_->declare_or_get_parameter(
57  name + ".external_detection_timeout", 1.0);
58  external_detection_translation_x_ = node_->declare_or_get_parameter(
59  name + ".external_detection_translation_x", -0.20);
60  external_detection_translation_y_ = node_->declare_or_get_parameter(
61  name + ".external_detection_translation_y", 0.0);
62  double yaw = node_->declare_or_get_parameter(
63  name + ".external_detection_rotation_yaw", 0.0);
64  double pitch = node_->declare_or_get_parameter(
65  name + ".external_detection_rotation_pitch", 1.57);
66  double roll = node_->declare_or_get_parameter(
67  name + ".external_detection_rotation_roll", -1.57);
68  double filter_coef = node_->declare_or_get_parameter(
69  name + ".filter_coef", 0.1);
70 
71  // Charging threshold from BatteryState message
72  charging_threshold_ = node_->declare_or_get_parameter(
73  name + ".charging_threshold", 0.5);
74 
75  // Optionally determine if docked via stall detection using joint_states
76  bool use_stall_detection = node_->declare_or_get_parameter(
77  name + ".use_stall_detection", false);
78  stall_joint_names_ = node_->declare_or_get_parameter(
79  name + ".stall_joint_names", std::vector<std::string>());
80  stall_velocity_threshold_ = node_->declare_or_get_parameter(
81  name + ".stall_velocity_threshold", 1.0);
82  stall_effort_threshold_ = node_->declare_or_get_parameter(
83  name + ".stall_effort_threshold", 1.0);
84 
85  // If not using stall detection, this is how close robot should get to pose
86  docking_threshold_ = node_->declare_or_get_parameter(
87  name + ".docking_threshold", 0.05);
88 
89  // Staging pose configuration
90  staging_x_offset_ = node_->declare_or_get_parameter(
91  name + ".staging_x_offset", -0.7);
92  staging_yaw_offset_ = node_->declare_or_get_parameter(
93  name + ".staging_yaw_offset", 0.0);
94 
95  // Direction of docking and if we should rotate to dock
96  std::string dock_direction = node_->declare_or_get_parameter(
97  name + ".dock_direction", std::string("forward"));
98  rotate_to_dock_ = node_->declare_or_get_parameter(
99  name + ".rotate_to_dock", false);
100 
101  transform_staleness_threshold_ = node_->declare_or_get_parameter(
102  name + ".transform_staleness_threshold", 0.0);
103 
104  node_->get_parameter("base_frame", base_frame_id_); // Get server base frame ID
105 
106  // Initialize detection state
107  detection_active_ = false;
108  initial_pose_received_ = false;
109 
110  // Create persistent subscription if toggling is disabled.
111  if (use_external_detection_pose_ && !subscribe_toggle_) {
112  dock_pose_.header.stamp = rclcpp::Time(0);
113  dock_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
114  "detected_dock_pose",
115  [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr & pose) {
116  detected_dock_pose_ = *pose;
117  initial_pose_received_ = true;
118  },
120  }
121 
122  dock_direction_ = utils::getDockDirectionFromString(dock_direction);
123  if (dock_direction_ == opennav_docking_core::DockDirection::UNKNOWN) {
124  throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"};
125  }
126 
127  if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) {
128  throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not "
129  "backward. Please set dock direction to backward."};
130  }
131 
132  // Setup filter
133  external_detection_rotation_.setRPY(roll, pitch, yaw);
134  filter_ = std::make_unique<PoseFilter>(filter_coef, external_detection_timeout_);
135 
136  if (!detector_service_name_.empty()) {
137  detector_client_ = node_->create_client<std_srvs::srv::Trigger>(
138  detector_service_name_, false);
139  }
140 
141  if (use_battery_status_) {
142  battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
143  "battery_state",
144  [this](const sensor_msgs::msg::BatteryState::ConstSharedPtr & state) {
145  is_charging_ = state->current > charging_threshold_;
146  });
147  }
148 
149  if (use_stall_detection) {
150  is_stalled_ = false;
151  if (stall_joint_names_.size() < 1) {
152  RCLCPP_ERROR(node_->get_logger(), "stall_joint_names cannot be empty!");
153  }
154  joint_state_sub_ = node_->create_subscription<sensor_msgs::msg::JointState>(
155  "joint_states",
156  std::bind(&SimpleChargingDock::jointStateCallback, this, std::placeholders::_1),
158  }
159 
160  dock_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
161  "dock_pose", nav2::qos::LatchedPublisherQoS());
162  filtered_dock_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
163  "filtered_dock_pose", nav2::qos::LatchedPublisherQoS());
164  staging_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
165  "staging_pose", nav2::qos::LatchedPublisherQoS());
166 }
167 
168 geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose(
169  const geometry_msgs::msg::Pose & pose, const std::string & frame)
170 {
171  // If not using detection, set the dock pose as the given dock pose estimate
172  if (!use_external_detection_pose_) {
173  // This gets called at the start of docking
174  // Reset our internally tracked dock pose
175  dock_pose_.header.frame_id = frame;
176  dock_pose_.pose = pose;
177  }
178 
179  // Compute the staging pose with given offsets
180  const double yaw = tf2::getYaw(pose.orientation);
181  geometry_msgs::msg::PoseStamped staging_pose;
182  staging_pose.header.frame_id = frame;
183  staging_pose.header.stamp = node_->now();
184  staging_pose.pose = pose;
185  staging_pose.pose.position.x += cos(yaw) * staging_x_offset_;
186  staging_pose.pose.position.y += sin(yaw) * staging_x_offset_;
187  tf2::Quaternion orientation;
188  orientation.setRPY(0.0, 0.0, yaw + staging_yaw_offset_);
189  staging_pose.pose.orientation = tf2::toMsg(orientation);
190 
191  // Publish staging pose for debugging purposes
192  staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
193  return staging_pose;
194 }
195 
196 bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string /*id*/)
197 {
198  // If using not detection, set the dock pose to the static fixed-frame version
199  if (!use_external_detection_pose_) {
200  dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
201  dock_pose_ = pose;
202  return true;
203  }
204 
205  // Guard against using pose data before the first detection has arrived.
206  if (!initial_pose_received_) {
207  RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet");
208  return false;
209  }
210 
211  // If using detections, get current detections, transform to frame, and apply offsets
212  geometry_msgs::msg::PoseStamped detected = detected_dock_pose_;
213 
214  // Validate that external pose is new enough
215  auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_);
216  if (node_->now() - detected.header.stamp > timeout) {
217  RCLCPP_WARN(
218  node_->get_logger(), "Lost detection or did not detect: "
219  "timeout exceeded (is %2.2f seconds old)",
220  static_cast<float>((node_->now() - detected.header.stamp).seconds()));
221  return false;
222  }
223 
224  // Transform detected pose into fixed frame. Note that the argument pose
225  // is the output of detection, but also acts as the initial estimate
226  // and contains the frame_id of docking
227  if (detected.header.frame_id != pose.header.frame_id) {
228  try {
229  if (rclcpp::Time(detected.header.stamp).nanoseconds() == 0) {
230  RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 3000,
231  "Provided a detection pose without a timestamp, unexpected");
232  }
233  geometry_msgs::msg::TransformStamped frame_transform = tf2_buffer_->lookupTransform(
234  pose.header.frame_id, detected.header.frame_id,
235  detected.header.stamp, rclcpp::Duration::from_seconds(0.2));
236  tf2::doTransform(detected, detected, frame_transform);
237  } catch (const tf2::TransformException & ex) {
238  RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose: %s", ex.what());
239  return false;
240  }
241  }
242 
243  // Filter the detected pose
244  detected = filter_->update(detected);
245  filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
246 
247  // Rotate the just the orientation, then remove roll/pitch
248  geometry_msgs::msg::PoseStamped just_orientation;
249  just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_);
250  geometry_msgs::msg::TransformStamped transform;
251  transform.transform.rotation = detected.pose.orientation;
252  tf2::doTransform(just_orientation, just_orientation, transform);
253 
254  tf2::Quaternion orientation;
255  orientation.setRPY(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation));
256  dock_pose_.pose.orientation = tf2::toMsg(orientation);
257 
258  // Construct dock_pose_ by applying translation/rotation
259  dock_pose_.header = detected.header;
260  dock_pose_.pose.position = detected.pose.position;
261  const double yaw = tf2::getYaw(dock_pose_.pose.orientation);
262  dock_pose_.pose.position.x += cos(yaw) * external_detection_translation_x_ -
263  sin(yaw) * external_detection_translation_y_;
264  dock_pose_.pose.position.y += sin(yaw) * external_detection_translation_x_ +
265  cos(yaw) * external_detection_translation_y_;
266  dock_pose_.pose.position.z = 0.0;
267 
268  // Publish & return dock pose for debugging purposes
269  dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
270  pose = dock_pose_;
271  return true;
272 }
273 
274 bool SimpleChargingDock::isDocked()
275 {
276  if (joint_state_sub_) {
277  // Using stall detection
278  return is_stalled_;
279  }
280 
281  if (dock_pose_.header.frame_id.empty()) {
282  // Dock pose is not yet valid
283  return false;
284  }
285 
286  geometry_msgs::msg::PoseStamped base_pose;
287  if (!nav2_util::getFreshPose(
288  *tf2_buffer_, dock_pose_.header.frame_id, base_frame_id_, node_->now(),
289  transform_staleness_threshold_, base_pose))
290  {
291  return false;
292  }
293 
294  // If we are close enough, pretend we are charging
295  double d = std::hypot(
296  base_pose.pose.position.x - dock_pose_.pose.position.x,
297  base_pose.pose.position.y - dock_pose_.pose.position.y);
298  return d < docking_threshold_;
299 }
300 
301 bool SimpleChargingDock::isCharging()
302 {
303  return use_battery_status_ ? is_charging_ : isDocked();
304 }
305 
306 bool SimpleChargingDock::disableCharging()
307 {
308  return true;
309 }
310 
311 bool SimpleChargingDock::hasStoppedCharging()
312 {
313  return !isCharging();
314 }
315 
316 void SimpleChargingDock::jointStateCallback(
317  const sensor_msgs::msg::JointState::ConstSharedPtr & state)
318 {
319  double velocity = 0.0;
320  double effort = 0.0;
321  for (size_t i = 0; i < state->name.size(); ++i) {
322  for (auto & name : stall_joint_names_) {
323  if (state->name[i] == name) {
324  // Tracking this joint
325  velocity += abs(state->velocity[i]);
326  effort += abs(state->effort[i]);
327  }
328  }
329  }
330 
331  // Take average
332  effort /= stall_joint_names_.size();
333  velocity /= stall_joint_names_.size();
334 
335  is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_);
336 }
337 
338 bool SimpleChargingDock::startDetectionProcess()
339 {
340  // Skip if already active
341  if (detection_active_) {
342  return true;
343  }
344 
345  // 1. Service START request
346  if (detector_client_) {
347  auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
348  try {
349  auto future = detector_client_->invoke(
350  req,
351  std::chrono::duration_cast<std::chrono::nanoseconds>(
352  std::chrono::duration<double>(detector_service_timeout_)));
353 
354  if (!future || !future->success) {
355  RCLCPP_ERROR(
356  node_->get_logger(), "Detector service '%s' failed to start.",
357  detector_service_name_.c_str());
358  return false;
359  }
360  } catch (const std::exception & e) {
361  RCLCPP_ERROR(
362  node_->get_logger(), "Calling detector service '%s' failed: %s",
363  detector_service_name_.c_str(), e.what());
364  return false;
365  }
366  }
367 
368  // 2. Subscription toggle
369  // Only subscribe once; will set state to ON on first message
370  if (subscribe_toggle_ && !dock_pose_sub_) {
371  dock_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
372  "detected_dock_pose",
373  [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr & pose) {
374  detected_dock_pose_ = *pose;
375  initial_pose_received_ = true;
376  },
378  }
379 
380  detection_active_ = true;
381  RCLCPP_INFO(node_->get_logger(), "External detector activation requested.");
382  return true;
383 }
384 
385 bool SimpleChargingDock::stopDetectionProcess()
386 {
387  // Skip if already OFF
388  if (!detection_active_) {
389  return true;
390  }
391 
392  // 1. Service STOP request
393  if (detector_client_) {
394  auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
395  try {
396  auto future = detector_client_->invoke(
397  req,
398  std::chrono::duration_cast<std::chrono::nanoseconds>(
399  std::chrono::duration<double>(detector_service_timeout_)));
400 
401  if (!future || !future->success) {
402  RCLCPP_ERROR(
403  node_->get_logger(), "Detector service '%s' failed to stop.",
404  detector_service_name_.c_str());
405  return false;
406  }
407  } catch (const std::exception & e) {
408  RCLCPP_ERROR(
409  node_->get_logger(), "Calling detector service '%s' failed: %s",
410  detector_service_name_.c_str(), e.what());
411  return false;
412  }
413  }
414 
415  // 2. Unsubscribe to release resources
416  // reset() will tear down the topic subscription immediately
417  if (subscribe_toggle_ && dock_pose_sub_) {
418  dock_pose_sub_.reset();
419  }
420 
421  detection_active_ = false;
422  initial_pose_received_ = false;
423  RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested.");
424  return true;
425 }
426 
427 void SimpleChargingDock::activate()
428 {
429  dock_pose_pub_->on_activate();
430  filtered_dock_pose_pub_->on_activate();
431  staging_pose_pub_->on_activate();
432 }
433 
434 void SimpleChargingDock::deactivate()
435 {
436  stopDetectionProcess();
437  dock_pose_pub_->on_deactivate();
438  filtered_dock_pose_pub_->on_deactivate();
439  staging_pose_pub_->on_deactivate();
440  RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock deactivated");
441 }
442 
443 void SimpleChargingDock::cleanup()
444 {
445  detector_client_.reset();
446  dock_pose_sub_.reset();
447  detection_active_ = false;
448  initial_pose_received_ = false;
449  RCLCPP_DEBUG(node_->get_logger(), "SimpleChargingDock cleaned up");
450 }
451 
452 } // namespace opennav_docking
453 
454 #include "pluginlib/class_list_macros.hpp"
A QoS profile for latched, reliable topics with a history of 1 messages.
A QoS profile for standard reliable topics with a history of 10 messages.
Abstract interface for a charging dock for the docking framework.