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"
24 using namespace std::chrono_literals;
26 namespace opennav_docking
29 void SimpleChargingDock::configure(
30 const nav2::LifecycleNode::WeakPtr & parent,
31 const std::string & name, nav2::TransformBuffer::SharedPtr tf)
36 node_ = parent.lock();
38 throw std::runtime_error{
"Failed to lock node"};
42 use_battery_status_ = node_->declare_or_get_parameter(
43 name +
".use_battery_status",
true);
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);
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);
72 charging_threshold_ = node_->declare_or_get_parameter(
73 name +
".charging_threshold", 0.5);
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);
86 docking_threshold_ = node_->declare_or_get_parameter(
87 name +
".docking_threshold", 0.05);
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);
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);
101 transform_staleness_threshold_ = node_->declare_or_get_parameter(
102 name +
".transform_staleness_threshold", 0.0);
104 node_->get_parameter(
"base_frame", base_frame_id_);
107 detection_active_ =
false;
108 initial_pose_received_ =
false;
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;
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"};
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."};
133 external_detection_rotation_.setRPY(roll, pitch, yaw);
134 filter_ = std::make_unique<PoseFilter>(filter_coef, external_detection_timeout_);
136 if (!detector_service_name_.empty()) {
137 detector_client_ = node_->create_client<std_srvs::srv::Trigger>(
138 detector_service_name_,
false);
141 if (use_battery_status_) {
142 battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
144 [
this](
const sensor_msgs::msg::BatteryState::ConstSharedPtr & state) {
145 is_charging_ = state->current > charging_threshold_;
149 if (use_stall_detection) {
151 if (stall_joint_names_.size() < 1) {
152 RCLCPP_ERROR(node_->get_logger(),
"stall_joint_names cannot be empty!");
154 joint_state_sub_ = node_->create_subscription<sensor_msgs::msg::JointState>(
156 std::bind(&SimpleChargingDock::jointStateCallback,
this, std::placeholders::_1),
160 dock_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
162 filtered_dock_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
164 staging_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
168 geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose(
169 const geometry_msgs::msg::Pose & pose,
const std::string & frame)
172 if (!use_external_detection_pose_) {
175 dock_pose_.header.frame_id = frame;
176 dock_pose_.pose = pose;
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);
192 staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
196 bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string )
199 if (!use_external_detection_pose_) {
200 dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
206 if (!initial_pose_received_) {
207 RCLCPP_WARN(node_->get_logger(),
"Waiting for first detected_dock_pose; none received yet");
212 geometry_msgs::msg::PoseStamped detected = detected_dock_pose_;
215 auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_);
216 if (node_->now() - detected.header.stamp > timeout) {
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()));
227 if (detected.header.frame_id != pose.header.frame_id) {
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");
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());
244 detected = filter_->update(detected);
245 filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
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);
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);
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;
269 dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
274 bool SimpleChargingDock::isDocked()
276 if (joint_state_sub_) {
281 if (dock_pose_.header.frame_id.empty()) {
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))
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_;
301 bool SimpleChargingDock::isCharging()
303 return use_battery_status_ ? is_charging_ : isDocked();
306 bool SimpleChargingDock::disableCharging()
311 bool SimpleChargingDock::hasStoppedCharging()
313 return !isCharging();
316 void SimpleChargingDock::jointStateCallback(
317 const sensor_msgs::msg::JointState::ConstSharedPtr & state)
319 double velocity = 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) {
325 velocity += abs(state->velocity[i]);
326 effort += abs(state->effort[i]);
332 effort /= stall_joint_names_.size();
333 velocity /= stall_joint_names_.size();
335 is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_);
338 bool SimpleChargingDock::startDetectionProcess()
341 if (detection_active_) {
346 if (detector_client_) {
347 auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
349 auto future = detector_client_->invoke(
351 std::chrono::duration_cast<std::chrono::nanoseconds>(
352 std::chrono::duration<double>(detector_service_timeout_)));
354 if (!future || !future->success) {
356 node_->get_logger(),
"Detector service '%s' failed to start.",
357 detector_service_name_.c_str());
360 }
catch (
const std::exception & e) {
362 node_->get_logger(),
"Calling detector service '%s' failed: %s",
363 detector_service_name_.c_str(), e.what());
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;
380 detection_active_ =
true;
381 RCLCPP_INFO(node_->get_logger(),
"External detector activation requested.");
385 bool SimpleChargingDock::stopDetectionProcess()
388 if (!detection_active_) {
393 if (detector_client_) {
394 auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
396 auto future = detector_client_->invoke(
398 std::chrono::duration_cast<std::chrono::nanoseconds>(
399 std::chrono::duration<double>(detector_service_timeout_)));
401 if (!future || !future->success) {
403 node_->get_logger(),
"Detector service '%s' failed to stop.",
404 detector_service_name_.c_str());
407 }
catch (
const std::exception & e) {
409 node_->get_logger(),
"Calling detector service '%s' failed: %s",
410 detector_service_name_.c_str(), e.what());
417 if (subscribe_toggle_ && dock_pose_sub_) {
418 dock_pose_sub_.reset();
421 detection_active_ =
false;
422 initial_pose_received_ =
false;
423 RCLCPP_INFO(node_->get_logger(),
"External detector deactivation requested.");
427 void SimpleChargingDock::activate()
429 dock_pose_pub_->on_activate();
430 filtered_dock_pose_pub_->on_activate();
431 staging_pose_pub_->on_activate();
434 void SimpleChargingDock::deactivate()
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");
443 void SimpleChargingDock::cleanup()
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");
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.