spin.cpp

// Copyright (c) 2018 Intel Corporation, 2019 Samsung Research America
//
// 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 <thread>
#include <algorithm>
#include <memory>
#include <utility>
 
#include "nav2_behaviors/plugins/spin.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
 
using namespace std::chrono_literals;
 
namespace nav2_behaviors
{
 
Spin::Spin()
: TimedBehavior<SpinAction>(),
  feedback_(std::make_shared<SpinAction::Feedback>()),
  min_rotational_vel_(0.0),
  max_rotational_vel_(0.0),
  rotational_acc_lim_(0.0),
  cmd_yaw_(0.0),
  prev_yaw_(0.0),
  relative_yaw_(0.0),
  simulate_ahead_time_(0.0)
{
}
 
Spin::~Spin() = default;
 
void Spin::onConfigure()
{
  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }
 
  simulate_ahead_time_ = node->declare_or_get_parameter(
    behavior_name_ + ".simulate_ahead_time", 2.0);
  max_rotational_vel_ = node->declare_or_get_parameter(
    behavior_name_ + ".max_rotational_vel", 1.0);
  min_rotational_vel_ = node->declare_or_get_parameter(
    behavior_name_ + ".min_rotational_vel", 0.4);
  rotational_acc_lim_ = node->declare_or_get_parameter(
    behavior_name_ + ".rotational_acc_lim", 3.2);
}
 
ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)
{
  geometry_msgs::msg::PoseStamped current_pose;
  if (!getCurrentPoseChecked(current_pose)) {
    std::string error_msg = "Current robot pose is not available.";
    RCLCPP_ERROR(logger_, "%s", error_msg.c_str());
    return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
  }
 
  prev_yaw_ = tf2::getYaw(current_pose.pose.orientation);
  relative_yaw_ = 0.0;
 
  cmd_yaw_ = command->target_yaw;
  RCLCPP_INFO(
    logger_, "Turning %0.2f for spin behavior.",
    cmd_yaw_);
 
  command_time_allowance_ = command->time_allowance;
  cmd_disable_collision_checks_ = command->disable_collision_checks;
  end_time_ = this->clock_->now() + command_time_allowance_;
 
  return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE, ""};
}
 
ResultStatus Spin::onCycleUpdate()
{
  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
  if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
    stopRobot();
    std::string error_msg = "Exceeded time allowance before reaching the Spin goal - Exiting Spin";
    RCLCPP_WARN(logger_, "%s", error_msg.c_str());
    return ResultStatus{Status::FAILED, SpinActionResult::TIMEOUT, error_msg};
  }
 
  geometry_msgs::msg::PoseStamped current_pose;
  if (!getCurrentPoseChecked(current_pose)) {
    stopRobot();
    std::string error_msg = "Current robot pose is not available.";
    RCLCPP_ERROR(logger_, "%s", error_msg.c_str());
    return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
  }
 
  const double current_yaw = tf2::getYaw(current_pose.pose.orientation);
 
  double delta_yaw = current_yaw - prev_yaw_;
  if (abs(delta_yaw) > M_PI) {
    delta_yaw = copysign(2 * M_PI - abs(delta_yaw), prev_yaw_);
  }
 
  relative_yaw_ += delta_yaw;
  prev_yaw_ = current_yaw;
 
  feedback_->angular_distance_traveled = static_cast<float>(relative_yaw_);
  action_server_->publish_feedback(feedback_);
 
  double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
  if (remaining_yaw < 1e-6) {
    stopRobot();
    return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE, ""};
  }
 
  double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
  vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
 
  auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
  cmd_vel->header.frame_id = robot_base_frame_;
  cmd_vel->header.stamp = clock_->now();
  cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_);
 
  geometry_msgs::msg::Pose pose = current_pose.pose;
 
  if (!isCollisionFree(relative_yaw_, cmd_vel->twist, pose)) {
    stopRobot();
    std::string error_msg = "Collision Ahead - Exiting Spin";
    RCLCPP_WARN(logger_, "%s", error_msg.c_str());
    return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD, error_msg};
  }
 
  vel_pub_->publish(std::move(cmd_vel));
 
  return ResultStatus{Status::RUNNING, SpinActionResult::NONE, ""};
}
 
bool Spin::isCollisionFree(
  const double & relative_yaw,
  const geometry_msgs::msg::Twist & cmd_vel,
  geometry_msgs::msg::Pose & pose)
{
  if (cmd_disable_collision_checks_) {
    return true;
  }
 
  // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
  int cycle_count = 0;
  double sim_position_change;
  const int max_cycle_count = static_cast<int>(cycle_frequency_ * simulate_ahead_time_);
  geometry_msgs::msg::Pose init_pose = pose;
  double init_theta = tf2::getYaw(init_pose.orientation);
  bool fetch_data = true;
 
  while (cycle_count < max_cycle_count) {
    sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_);
    double new_theta = init_theta + sim_position_change;
    pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(new_theta);
    cycle_count++;
 
    if (abs(relative_yaw) - abs(sim_position_change) <= 0.0) {
      break;
    }
 
    if (!local_collision_checker_->isCollisionFree(pose, fetch_data)) {
      return false;
    }
    fetch_data = false;
  }
  return true;
}
 
}  // namespace nav2_behaviors
 
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Spin, nav2_core::Behavior)