simple_non_charging_dock.cpp

// Copyright (c) 2024 Open Navigation LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include <cmath>
#include <chrono>
 
#include "nav2_ros_common/node_utils.hpp"
#include "opennav_docking/simple_non_charging_dock.hpp"
#include "opennav_docking/utils.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
 
using namespace std::chrono_literals;
 
namespace opennav_docking
{
 
void SimpleNonChargingDock::configure(
  const nav2::LifecycleNode::WeakPtr & parent,
  const std::string & name, nav2::TransformBuffer::SharedPtr tf)
{
  name_ = name;
  tf2_buffer_ = tf;
  node_ = parent.lock();
  if (!node_) {
    throw std::runtime_error{"Failed to lock node"};
  }
 
  // Parameters for optional external detection of dock pose
  use_external_detection_pose_ = node_->declare_or_get_parameter(
    name + ".use_external_detection_pose", false);
  external_detection_timeout_ = node_->declare_or_get_parameter(
    name + ".external_detection_timeout", 1.0);
  external_detection_translation_x_ = node_->declare_or_get_parameter(
    name + ".external_detection_translation_x", -0.20);
  external_detection_translation_y_ = node_->declare_or_get_parameter(
    name + ".external_detection_translation_y", 0.0);
  double yaw = node_->declare_or_get_parameter(
    name + ".external_detection_rotation_yaw", 0.0);
  double pitch = node_->declare_or_get_parameter(
    name + ".external_detection_rotation_pitch", 1.57);
  double roll = node_->declare_or_get_parameter(
    name + ".external_detection_rotation_roll", -1.57);
  double filter_coef = node_->declare_or_get_parameter(
    name + ".filter_coef", 0.1);
 
  // Parameters for optional detector control
  detector_service_name_ = node_->declare_or_get_parameter(
    name + ".detector_service_name", std::string(""));
  detector_service_timeout_ = node_->declare_or_get_parameter(
    name + ".detector_service_timeout", 5.0);
  subscribe_toggle_ = node_->declare_or_get_parameter(
    name + ".subscribe_toggle", false);
 
  // Optionally determine if docked via stall detection using joint_states
  bool use_stall_detection = node_->declare_or_get_parameter(
    name + ".use_stall_detection", false);
  stall_joint_names_ = node_->declare_or_get_parameter(
    name + ".stall_joint_names", std::vector<std::string>());
  stall_velocity_threshold_ = node_->declare_or_get_parameter(
    name + ".stall_velocity_threshold", 1.0);
  stall_effort_threshold_ = node_->declare_or_get_parameter(
    name + ".stall_effort_threshold", 1.0);
 
  // If not using stall detection, this is how close robot should get to pose
  docking_threshold_ = node_->declare_or_get_parameter(
    name + ".docking_threshold", 0.05);
 
  // Staging pose configuration
  staging_x_offset_ = node_->declare_or_get_parameter(
    name + ".staging_x_offset", -0.7);
  staging_yaw_offset_ = node_->declare_or_get_parameter(
    name + ".staging_yaw_offset", 0.0);
 
  // Direction of docking and if we should rotate to dock
  std::string dock_direction = node_->declare_or_get_parameter(
    name + ".dock_direction", std::string("forward"));
  rotate_to_dock_ = node_->declare_or_get_parameter(
    name + ".rotate_to_dock", false);
 
  node_->get_parameter("base_frame", base_frame_id_);  // Get server base frame ID
 
  // Initialize detection state
  detection_active_ = false;
  initial_pose_received_ = false;
 
  // Create persistent subscription if toggling is disabled.
  if (use_external_detection_pose_ && !subscribe_toggle_) {
    dock_pose_.header.stamp = rclcpp::Time(0);
    dock_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
      "detected_dock_pose",
      [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr & pose) {
        detected_dock_pose_ = *pose;
        initial_pose_received_ = true;
      },
      nav2::qos::StandardTopicQoS());
  }
 
  dock_direction_ = utils::getDockDirectionFromString(dock_direction);
  if (dock_direction_ == opennav_docking_core::DockDirection::UNKNOWN) {
    throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"};
  }
 
  if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) {
    throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not "
            "backward. Please set dock direction to backward."};
  }
 
  // Setup filter
  external_detection_rotation_.setRPY(roll, pitch, yaw);
  filter_ = std::make_unique<PoseFilter>(filter_coef, external_detection_timeout_);
 
  if (!detector_service_name_.empty()) {
    detector_client_ = node_->create_client<std_srvs::srv::Trigger>(
      detector_service_name_, false);
  }
 
  if (use_stall_detection) {
    is_stalled_ = false;
    if (stall_joint_names_.size() < 1) {
      RCLCPP_ERROR(node_->get_logger(), "stall_joint_names cannot be empty!");
    }
    joint_state_sub_ = node_->create_subscription<sensor_msgs::msg::JointState>(
      "joint_states",
      std::bind(&SimpleNonChargingDock::jointStateCallback, this, std::placeholders::_1),
      nav2::qos::StandardTopicQoS());
  }
 
  dock_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
    "dock_pose", nav2::qos::LatchedPublisherQoS());
  filtered_dock_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
    "filtered_dock_pose", nav2::qos::LatchedPublisherQoS());
  staging_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>(
    "staging_pose", nav2::qos::LatchedPublisherQoS());
}
 
geometry_msgs::msg::PoseStamped SimpleNonChargingDock::getStagingPose(
  const geometry_msgs::msg::Pose & pose, const std::string & frame)
{
  // If not using detection, set the dock pose as the given dock pose estimate
  if (!use_external_detection_pose_) {
    // This gets called at the start of docking
    // Reset our internally tracked dock pose
    dock_pose_.header.frame_id = frame;
    dock_pose_.pose = pose;
  }
 
  // Compute the staging pose with given offsets
  const double yaw = tf2::getYaw(pose.orientation);
  geometry_msgs::msg::PoseStamped staging_pose;
  staging_pose.header.frame_id = frame;
  staging_pose.header.stamp = node_->now();
  staging_pose.pose = pose;
  staging_pose.pose.position.x += cos(yaw) * staging_x_offset_;
  staging_pose.pose.position.y += sin(yaw) * staging_x_offset_;
  tf2::Quaternion orientation;
  orientation.setRPY(0.0, 0.0, yaw + staging_yaw_offset_);
  staging_pose.pose.orientation = tf2::toMsg(orientation);
 
  // Publish staging pose for debugging purposes
  staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
  return staging_pose;
}
 
bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string)
{
  // If using not detection, set the dock pose to the static fixed-frame version
  if (!use_external_detection_pose_) {
    dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
    dock_pose_ = pose;
    return true;
  }
 
  // Guard against using pose data before the first detection has arrived.
  if (!initial_pose_received_) {
    RCLCPP_WARN(node_->get_logger(), "Waiting for first detected_dock_pose; none received yet");
    return false;
  }
 
  // If using detections, get current detections, transform to frame, and apply offsets
  geometry_msgs::msg::PoseStamped detected = detected_dock_pose_;
 
  // Validate that external pose is new enough
  auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_);
  if (node_->now() - detected.header.stamp > timeout) {
    RCLCPP_WARN(node_->get_logger(), "Lost detection or did not detect: timeout exceeded");
    return false;
  }
 
  // Transform detected pose into fixed frame. Note that the argument pose
  // is the output of detection, but also acts as the initial estimate
  // and contains the frame_id of docking
  if (detected.header.frame_id != pose.header.frame_id) {
    try {
      if (!tf2_buffer_->canTransform(
          pose.header.frame_id, detected.header.frame_id,
          detected.header.stamp, rclcpp::Duration::from_seconds(0.2)))
      {
        RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose");
        return false;
      }
      tf2_buffer_->transform(detected, detected, pose.header.frame_id);
    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose");
      return false;
    }
  }
 
  // Filter the detected pose
  detected = filter_->update(detected);
  filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
 
  // Rotate the just the orientation, then remove roll/pitch
  geometry_msgs::msg::PoseStamped just_orientation;
  just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_);
  geometry_msgs::msg::TransformStamped transform;
  transform.transform.rotation = detected.pose.orientation;
  tf2::doTransform(just_orientation, just_orientation, transform);
 
  tf2::Quaternion orientation;
  orientation.setRPY(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation));
  dock_pose_.pose.orientation = tf2::toMsg(orientation);
 
  // Construct dock_pose_ by applying translation/rotation
  dock_pose_.header = detected.header;
  dock_pose_.pose.position = detected.pose.position;
  const double yaw = tf2::getYaw(dock_pose_.pose.orientation);
  dock_pose_.pose.position.x += cos(yaw) * external_detection_translation_x_ -
    sin(yaw) * external_detection_translation_y_;
  dock_pose_.pose.position.y += sin(yaw) * external_detection_translation_x_ +
    cos(yaw) * external_detection_translation_y_;
  dock_pose_.pose.position.z = 0.0;
 
  // Publish & return dock pose for debugging purposes
  dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
  pose = dock_pose_;
  return true;
}
 
bool SimpleNonChargingDock::isDocked()
{
  if (joint_state_sub_) {
    // Using stall detection
    return is_stalled_;
  }
 
  if (dock_pose_.header.frame_id.empty()) {
    // Dock pose is not yet valid
    return false;
  }
 
  // Find base pose in target frame
  geometry_msgs::msg::PoseStamped base_pose;
  base_pose.header.stamp = rclcpp::Time(0);
  base_pose.header.frame_id = base_frame_id_;
  base_pose.pose.orientation.w = 1.0;
  try {
    tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id);
  } catch (const tf2::TransformException & ex) {
    return false;
  }
 
  // If we are close enough, we are docked
  double d = std::hypot(
    base_pose.pose.position.x - dock_pose_.pose.position.x,
    base_pose.pose.position.y - dock_pose_.pose.position.y);
  return d < docking_threshold_;
}
 
void SimpleNonChargingDock::jointStateCallback(
  const sensor_msgs::msg::JointState::ConstSharedPtr & state)
{
  double velocity = 0.0;
  double effort = 0.0;
  for (size_t i = 0; i < state->name.size(); ++i) {
    for (auto & name : stall_joint_names_) {
      if (state->name[i] == name) {
        // Tracking this joint
        velocity += abs(state->velocity[i]);
        effort += abs(state->effort[i]);
      }
    }
  }
 
  // Take average
  effort /= stall_joint_names_.size();
  velocity /= stall_joint_names_.size();
 
  is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_);
}
 
bool SimpleNonChargingDock::startDetectionProcess()
{
  // Skip if already active
  if (detection_active_) {
    return true;
  }
 
  // 1. Service START request
  if (detector_client_) {
    auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
    try {
      auto future = detector_client_->invoke(
        req,
        std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::duration<double>(detector_service_timeout_)));
 
      if (!future || !future->success) {
        RCLCPP_ERROR(
          node_->get_logger(), "Detector service '%s' failed to start.",
          detector_service_name_.c_str());
        return false;
      }
    } catch (const std::exception & e) {
      RCLCPP_ERROR(
        node_->get_logger(), "Calling detector service '%s' failed: %s",
        detector_service_name_.c_str(), e.what());
      return false;
    }
  }
 
  // 2. Subscription toggle
  //    Only subscribe once; will set state to ON on first message
  if (subscribe_toggle_ && !dock_pose_sub_) {
    dock_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
      "detected_dock_pose",
      [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr & pose) {
        detected_dock_pose_ = *pose;
        initial_pose_received_ = true;
      },
      nav2::qos::StandardTopicQoS());
  }
 
  detection_active_ = true;
  RCLCPP_INFO(node_->get_logger(), "External detector activation requested.");
  return true;
}
 
bool SimpleNonChargingDock::stopDetectionProcess()
{
  // Skip if already OFF
  if (!detection_active_) {
    return true;
  }
 
  // 1. Service STOP request
  if (detector_client_) {
    auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
    try {
      auto future = detector_client_->invoke(
        req,
        std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::duration<double>(detector_service_timeout_)));
 
      if (!future || !future->success) {
        RCLCPP_ERROR(
          node_->get_logger(), "Detector service '%s' failed to stop.",
          detector_service_name_.c_str());
        return false;
      }
    } catch (const std::exception & e) {
      RCLCPP_ERROR(
        node_->get_logger(), "Calling detector service '%s' failed: %s",
        detector_service_name_.c_str(), e.what());
      return false;
    }
  }
 
  // 2. Unsubscribe to release resources
  //    reset() will tear down the topic subscription immediately
  if (subscribe_toggle_ && dock_pose_sub_) {
    dock_pose_sub_.reset();
  }
 
  detection_active_ = false;
  initial_pose_received_ = false;
  RCLCPP_INFO(node_->get_logger(), "External detector deactivation requested.");
  return true;
}
 
void SimpleNonChargingDock::activate()
{
  dock_pose_pub_->on_activate();
  filtered_dock_pose_pub_->on_activate();
  staging_pose_pub_->on_activate();
}
 
void SimpleNonChargingDock::deactivate()
{
  stopDetectionProcess();
  dock_pose_pub_->on_deactivate();
  filtered_dock_pose_pub_->on_deactivate();
  staging_pose_pub_->on_deactivate();
  RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock deactivated");
}
 
void SimpleNonChargingDock::cleanup()
{
  detector_client_.reset();
  dock_pose_sub_.reset();
  detection_active_ = false;
  initial_pose_received_ = false;
  RCLCPP_DEBUG(node_->get_logger(), "SimpleNonChargingDock cleaned up");
}
 
}  // namespace opennav_docking
 
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(opennav_docking::SimpleNonChargingDock, opennav_docking_core::ChargingDock)