Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
assisted_teleop.cpp
1 // Copyright (c) 2022 Joshua Wallace
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 <utility>
16 
17 #include "nav2_behaviors/plugins/assisted_teleop.hpp"
18 #include "nav2_ros_common/node_utils.hpp"
19 #include "nav2_util/geometry_utils.hpp"
20 
21 namespace nav2_behaviors
22 {
23 AssistedTeleop::AssistedTeleop()
24 : TimedBehavior<AssistedTeleopAction>(),
25  feedback_(std::make_shared<AssistedTeleopAction::Feedback>())
26 {}
27 
28 void AssistedTeleop::onConfigure()
29 {
30  auto node = node_.lock();
31  if (!node) {
32  throw std::runtime_error{"Failed to lock node"};
33  }
34 
35  // set up parameters
36  projection_time_ = node->declare_or_get_parameter(
37  behavior_name_ + ".projection_time", 1.0);
38  simulation_time_step_ = node->declare_or_get_parameter(
39  behavior_name_ + ".simulation_time_step", 0.1);
40  teleop_command_timeout_ = node->declare_or_get_parameter(
41  behavior_name_ + ".teleop_command_timeout", 0.25);
42  std::string cmd_vel_teleop = node->declare_or_get_parameter(
43  behavior_name_ + ".cmd_vel_teleop", std::string("cmd_vel_teleop"));
44 
45  vel_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
46  node,
47  cmd_vel_teleop,
48  [&](const geometry_msgs::msg::Twist::ConstSharedPtr & msg) {
49  teleop_twist_.twist = *msg;
50  teleop_twist_.header.stamp = clock_->now();
51  received_first_command_ = true;
52  }, [&](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & msg) {
53  teleop_twist_ = *msg;
54  received_first_command_ = true;
55  });
56 
57  preempt_teleop_sub_ = node->create_subscription<std_msgs::msg::Empty>(
58  "preempt_teleop",
59  std::bind(
60  &AssistedTeleop::preemptTeleopCallback,
61  this, std::placeholders::_1));
62 }
63 
64 ResultStatus AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAction::Goal> command)
65 {
66  preempt_teleop_ = false;
67  received_first_command_ = false;
68  command_time_allowance_ = command->time_allowance;
69  end_time_ = this->clock_->now() + command_time_allowance_;
70  return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE, ""};
71 }
72 
73 void AssistedTeleop::onActionCompletion(std::shared_ptr<AssistedTeleopActionResult>/*result*/)
74 {
75  teleop_twist_ = geometry_msgs::msg::TwistStamped();
76  received_first_command_ = false;
77  preempt_teleop_ = false;
78 }
79 
80 ResultStatus AssistedTeleop::onCycleUpdate()
81 {
82  feedback_->current_teleop_duration = elapsed_time_;
83  action_server_->publish_feedback(feedback_);
84 
85  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
86  if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
87  std::string error_msg = "Exceeded time allowance before reaching the " + behavior_name_ +
88  "goal - Exiting " + behavior_name_;
89  RCLCPP_WARN_STREAM(logger_, error_msg.c_str());
90  return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TIMEOUT, error_msg};
91  }
92 
93  // user states that teleop was successful
94  if (preempt_teleop_) {
95  stopRobot();
96  return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE, ""};
97  }
98 
99  // teleop source stopped publishing (operator released input, node or link died)
100  if (isTeleopCommandStale(this->clock_->now())) {
101  std::string error_msg = "No teleop command received within teleop_command_timeout (" +
102  std::to_string(teleop_command_timeout_) + " s) - Exiting " + behavior_name_;
103  RCLCPP_WARN_STREAM(logger_, error_msg.c_str());
104  return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TELEOP_INPUT_TIMEOUT,
105  error_msg};
106  }
107 
108  geometry_msgs::msg::PoseStamped current_pose;
109  if (!getCurrentPoseChecked(current_pose)) {
110  std::string error_msg = "Current robot pose is not available for " + behavior_name_;
111  RCLCPP_ERROR_STREAM(logger_, error_msg.c_str());
112  return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TF_ERROR, error_msg};
113  }
114 
115  geometry_msgs::msg::Pose projected_pose = current_pose.pose;
116 
117  auto scaled_twist = std::make_unique<geometry_msgs::msg::TwistStamped>(teleop_twist_);
118  bool fetch_data = true;
119  for (double time = simulation_time_step_; time < projection_time_;
120  time += simulation_time_step_)
121  {
122  projected_pose = projectPose(projected_pose, teleop_twist_.twist, simulation_time_step_);
123 
124  if (!local_collision_checker_->isCollisionFree(projected_pose, fetch_data)) {
125  if (time == simulation_time_step_) {
126  RCLCPP_DEBUG_STREAM_THROTTLE(
127  logger_,
128  *clock_,
129  1000,
130  behavior_name_.c_str() << " collided on first time step, setting velocity to zero");
131  scaled_twist->twist.linear.x = 0.0f;
132  scaled_twist->twist.linear.y = 0.0f;
133  scaled_twist->twist.angular.z = 0.0f;
134  break;
135  } else {
136  RCLCPP_DEBUG_STREAM_THROTTLE(
137  logger_,
138  *clock_,
139  1000,
140  behavior_name_.c_str() << " collision approaching in " << time << " seconds");
141  double scale_factor = time / projection_time_;
142  scaled_twist->twist.linear.x *= scale_factor;
143  scaled_twist->twist.linear.y *= scale_factor;
144  scaled_twist->twist.angular.z *= scale_factor;
145  break;
146  }
147  }
148  fetch_data = false;
149  }
150  vel_pub_->publish(std::move(scaled_twist));
151 
152  return ResultStatus{Status::RUNNING, AssistedTeleopActionResult::NONE, ""};
153 }
154 
155 geometry_msgs::msg::Pose AssistedTeleop::projectPose(
156  const geometry_msgs::msg::Pose & pose,
157  const geometry_msgs::msg::Twist & twist,
158  double projection_time)
159 {
160  geometry_msgs::msg::Pose projected_pose = pose;
161 
162  double theta = tf2::getYaw(pose.orientation);
163 
164  projected_pose.position.x += projection_time * (
165  twist.linear.x * cos(theta) -
166  twist.linear.y * sin(theta));
167 
168  projected_pose.position.y += projection_time * (
169  twist.linear.x * sin(theta) +
170  twist.linear.y * cos(theta));
171 
172  double new_theta = theta + projection_time * twist.angular.z;
173  projected_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(new_theta);
174 
175  return projected_pose;
176 }
177 
178 void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::ConstSharedPtr &)
179 {
180  preempt_teleop_ = true;
181 }
182 
183 bool AssistedTeleop::isTeleopCommandStale(const rclcpp::Time & now) const
184 {
185  if (teleop_command_timeout_ <= 0.0 || !received_first_command_) {
186  return false;
187  }
188  return (now - rclcpp::Time(teleop_twist_.header.stamp, now.get_clock_type())).seconds() >
189  teleop_command_timeout_;
190 }
191 
192 } // namespace nav2_behaviors
193 
194 #include "pluginlib/class_list_macros.hpp"
An action server behavior for assisted teleop.
Abstract interface for behaviors to adhere to with pluginlib.
Definition: behavior.hpp:41