drive_on_heading.hpp

// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2022 Joshua Wallace
//
// 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.
 
#ifndef NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
#define NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
 
#include <chrono>
#include <memory>
#include <utility>
 
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/drive_on_heading.hpp"
#include "nav2_msgs/action/back_up.hpp"
#include "nav2_util/node_utils.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
 
namespace nav2_behaviors
{
 
template<typename ActionT = nav2_msgs::action::DriveOnHeading>
class DriveOnHeading : public TimedBehavior<ActionT>
{
  using CostmapInfoType = nav2_core::CostmapInfoType;
 
public:
  DriveOnHeading()
  : TimedBehavior<ActionT>(),
    feedback_(std::make_shared<typename ActionT::Feedback>()),
    command_x_(0.0),
    command_speed_(0.0),
    simulate_ahead_time_(0.0)
  {
  }
 
  ~DriveOnHeading() = default;
 
  ResultStatus onRun(const std::shared_ptr<const typename ActionT::Goal> command) override
  {
    if (command->target.y != 0.0 || command->target.z != 0.0) {
      RCLCPP_INFO(
        this->logger_,
        "DrivingOnHeading in Y and Z not supported, will only move in X.");
      return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT};
    }
 
    // Ensure that both the speed and direction have the same sign
    if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) {
      RCLCPP_ERROR(this->logger_, "Speed and command sign did not match");
      return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT};
    }
 
    command_x_ = command->target.x;
    command_speed_ = command->speed;
    command_time_allowance_ = command->time_allowance;
 
    end_time_ = this->clock_->now() + command_time_allowance_;
 
    if (!nav2_util::getCurrentPose(
        initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_,
        this->transform_tolerance_))
    {
      RCLCPP_ERROR(this->logger_, "Initial robot pose is not available.");
      return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
    }
 
    return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
  }
 
  ResultStatus onCycleUpdate() override
  {
    rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
    if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
      this->stopRobot();
      RCLCPP_WARN(
        this->logger_,
        "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
      return ResultStatus{Status::FAILED, ActionT::Result::TIMEOUT};
    }
 
    geometry_msgs::msg::PoseStamped current_pose;
    if (!nav2_util::getCurrentPose(
        current_pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
        this->transform_tolerance_))
    {
      RCLCPP_ERROR(this->logger_, "Current robot pose is not available.");
      return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
    }
 
    double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x;
    double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y;
    double distance = hypot(diff_x, diff_y);
 
    feedback_->distance_traveled = distance;
    this->action_server_->publish_feedback(feedback_);
 
    if (distance >= std::fabs(command_x_)) {
      this->stopRobot();
      return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
    }
 
    auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
    cmd_vel->header.stamp = this->clock_->now();
    cmd_vel->header.frame_id = this->robot_base_frame_;
    cmd_vel->twist.linear.y = 0.0;
    cmd_vel->twist.angular.z = 0.0;
    cmd_vel->twist.linear.x = command_speed_;
 
    geometry_msgs::msg::Pose2D pose2d;
    pose2d.x = current_pose.pose.position.x;
    pose2d.y = current_pose.pose.position.y;
    pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
 
    if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) {
      this->stopRobot();
      RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
      return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
    }
 
    this->vel_pub_->publish(std::move(cmd_vel));
 
    return ResultStatus{Status::RUNNING, ActionT::Result::NONE};
  }
 
  CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
 
protected:
  bool isCollisionFree(
    const double & distance,
    const geometry_msgs::msg::Twist & cmd_vel,
    geometry_msgs::msg::Pose2D & pose2d)
  {
    // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
    int cycle_count = 0;
    double sim_position_change;
    const double diff_dist = abs(command_x_) - distance;
    const int max_cycle_count = static_cast<int>(this->cycle_frequency_ * simulate_ahead_time_);
    geometry_msgs::msg::Pose2D init_pose = pose2d;
    bool fetch_data = true;
 
    while (cycle_count < max_cycle_count) {
      sim_position_change = cmd_vel.linear.x * (cycle_count / this->cycle_frequency_);
      pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta);
      pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta);
      cycle_count++;
 
      if (diff_dist - abs(sim_position_change) <= 0.) {
        break;
      }
 
      if (!this->local_collision_checker_->isCollisionFree(pose2d, fetch_data)) {
        return false;
      }
      fetch_data = false;
    }
    return true;
  }
 
  void onConfigure() override
  {
    auto node = this->node_.lock();
    if (!node) {
      throw std::runtime_error{"Failed to lock node"};
    }
 
    nav2_util::declare_parameter_if_not_declared(
      node,
      "simulate_ahead_time", rclcpp::ParameterValue(2.0));
    node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
  }
 
  typename ActionT::Feedback::SharedPtr feedback_;
 
  geometry_msgs::msg::PoseStamped initial_pose_;
  double command_x_;
  double command_speed_;
  rclcpp::Duration command_time_allowance_{0, 0};
  rclcpp::Time end_time_;
  double simulate_ahead_time_;
};
 
}  // namespace nav2_behaviors
 
#endif  // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_