Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
spin.cpp
1 // Copyright (c) 2018 Intel Corporation, 2019 Samsung Research America
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 <thread>
17 #include <algorithm>
18 #include <memory>
19 #include <utility>
20 
21 #include "nav2_behaviors/plugins/spin.hpp"
22 #include "nav2_ros_common/node_utils.hpp"
23 #include "nav2_util/geometry_utils.hpp"
24 
25 using namespace std::chrono_literals;
26 
27 namespace nav2_behaviors
28 {
29 
30 Spin::Spin()
31 : TimedBehavior<SpinAction>(),
32  feedback_(std::make_shared<SpinAction::Feedback>()),
33  min_rotational_vel_(0.0),
34  max_rotational_vel_(0.0),
35  rotational_acc_lim_(0.0),
36  cmd_yaw_(0.0),
37  prev_yaw_(0.0),
38  relative_yaw_(0.0),
39  simulate_ahead_time_(0.0)
40 {
41 }
42 
43 Spin::~Spin() = default;
44 
46 {
47  auto node = node_.lock();
48  if (!node) {
49  throw std::runtime_error{"Failed to lock node"};
50  }
51 
52  simulate_ahead_time_ = node->declare_or_get_parameter(
53  behavior_name_ + ".simulate_ahead_time", 2.0);
54  max_rotational_vel_ = node->declare_or_get_parameter(
55  behavior_name_ + ".max_rotational_vel", 1.0);
56  min_rotational_vel_ = node->declare_or_get_parameter(
57  behavior_name_ + ".min_rotational_vel", 0.4);
58  rotational_acc_lim_ = node->declare_or_get_parameter(
59  behavior_name_ + ".rotational_acc_lim", 3.2);
60 }
61 
62 ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)
63 {
64  geometry_msgs::msg::PoseStamped current_pose;
65  if (!getCurrentPoseChecked(current_pose)) {
66  std::string error_msg = "Current robot pose is not available.";
67  RCLCPP_ERROR(logger_, "%s", error_msg.c_str());
68  return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
69  }
70 
71  prev_yaw_ = tf2::getYaw(current_pose.pose.orientation);
72  relative_yaw_ = 0.0;
73 
74  cmd_yaw_ = command->target_yaw;
75  RCLCPP_INFO(
76  logger_, "Turning %0.2f for spin behavior.",
77  cmd_yaw_);
78 
79  command_time_allowance_ = command->time_allowance;
80  cmd_disable_collision_checks_ = command->disable_collision_checks;
81  end_time_ = this->clock_->now() + command_time_allowance_;
82 
83  return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE, ""};
84 }
85 
87 {
88  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
89  if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
90  std::string error_msg = "Exceeded time allowance before reaching the Spin goal - Exiting Spin";
91  RCLCPP_WARN(logger_, "%s", error_msg.c_str());
92  return ResultStatus{Status::FAILED, SpinActionResult::TIMEOUT, error_msg};
93  }
94 
95  geometry_msgs::msg::PoseStamped current_pose;
96  if (!getCurrentPoseChecked(current_pose)) {
97  std::string error_msg = "Current robot pose is not available.";
98  RCLCPP_ERROR(logger_, "%s", error_msg.c_str());
99  return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
100  }
101 
102  const double current_yaw = tf2::getYaw(current_pose.pose.orientation);
103 
104  double delta_yaw = current_yaw - prev_yaw_;
105  if (abs(delta_yaw) > M_PI) {
106  delta_yaw = copysign(2 * M_PI - abs(delta_yaw), prev_yaw_);
107  }
108 
109  relative_yaw_ += delta_yaw;
110  prev_yaw_ = current_yaw;
111 
112  feedback_->angular_distance_traveled = static_cast<float>(relative_yaw_);
113  action_server_->publish_feedback(feedback_);
114 
115  double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
116  if (remaining_yaw < 1e-6) {
117  stopRobot();
118  return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE, ""};
119  }
120 
121  double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
122  vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
123 
124  auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
125  cmd_vel->header.frame_id = robot_base_frame_;
126  cmd_vel->header.stamp = clock_->now();
127  cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_);
128 
129  geometry_msgs::msg::Pose pose = current_pose.pose;
130 
131  if (!isCollisionFree(remaining_yaw, cmd_vel->twist, pose)) {
132  std::string error_msg = "Collision Ahead - Exiting Spin";
133  RCLCPP_WARN(logger_, "%s", error_msg.c_str());
134  return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD, error_msg};
135  }
136 
137  vel_pub_->publish(std::move(cmd_vel));
138 
139  return ResultStatus{Status::RUNNING, SpinActionResult::NONE, ""};
140 }
141 
143  const double & relative_yaw,
144  const geometry_msgs::msg::Twist & cmd_vel,
145  geometry_msgs::msg::Pose & pose)
146 {
147  if (cmd_disable_collision_checks_) {
148  return true;
149  }
150 
151  // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
152  int cycle_count = 0;
153  double sim_position_change;
154  const int max_cycle_count = static_cast<int>(cycle_frequency_ * simulate_ahead_time_);
155  geometry_msgs::msg::Pose init_pose = pose;
156  double init_theta = tf2::getYaw(init_pose.orientation);
157  bool fetch_data = true;
158 
159  while (cycle_count < max_cycle_count) {
160  sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_);
161  double new_theta = init_theta + sim_position_change;
162  pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(new_theta);
163  cycle_count++;
164 
165  if (abs(relative_yaw) - abs(sim_position_change) <= 0.0) {
166  break;
167  }
168 
169  if (!local_collision_checker_->isCollisionFree(pose, fetch_data)) {
170  return false;
171  }
172  fetch_data = false;
173  }
174  return true;
175 }
176 
177 } // namespace nav2_behaviors
178 
179 #include "pluginlib/class_list_macros.hpp"
180 PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Spin, nav2_core::Behavior)
An action server behavior for spinning in.
Definition: spin.hpp:36
ResultStatus onRun(const std::shared_ptr< const SpinActionGoal > command) override
Initialization to run behavior.
Definition: spin.cpp:62
ResultStatus onCycleUpdate() override
Loop function to run behavior.
Definition: spin.cpp:86
void onConfigure() override
Configuration of behavior action.
Definition: spin.cpp:45
bool isCollisionFree(const double &distance, const geometry_msgs::msg::Twist &cmd_vel, geometry_msgs::msg::Pose &pose)
Check if pose is collision free.
Definition: spin.cpp:142
bool getCurrentPoseChecked(geometry_msgs::msg::PoseStamped &pose)
Get one checked robot pose in the local frame, preserving the TF timestamp.
Abstract interface for behaviors to adhere to with pluginlib.
Definition: behavior.hpp:41