add humble-navigation2

navigation2/nav2_behaviors/plugins/assisted_teleop.cpp

@@ -0,0 +1,186 @@
+// 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.
+
+#include <utility>
+
+#include "nav2_behaviors/plugins/assisted_teleop.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_behaviors
+{
+AssistedTeleop::AssistedTeleop()
+: TimedBehavior<AssistedTeleopAction>(),
+  feedback_(std::make_shared<AssistedTeleopAction::Feedback>())
+{}
+
+void AssistedTeleop::onConfigure()
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // set up parameters
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "projection_time", rclcpp::ParameterValue(1.0));
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "simulation_time_step", rclcpp::ParameterValue(0.1));
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "cmd_vel_teleop", rclcpp::ParameterValue(std::string("cmd_vel_teleop")));
+
+  node->get_parameter("projection_time", projection_time_);
+  node->get_parameter("simulation_time_step", simulation_time_step_);
+
+  std::string cmd_vel_teleop;
+  node->get_parameter("cmd_vel_teleop", cmd_vel_teleop);
+
+  vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
+    cmd_vel_teleop, rclcpp::SystemDefaultsQoS(),
+    std::bind(
+      &AssistedTeleop::teleopVelocityCallback,
+      this, std::placeholders::_1));
+
+  preempt_teleop_sub_ = node->create_subscription<std_msgs::msg::Empty>(
+    "preempt_teleop", rclcpp::SystemDefaultsQoS(),
+    std::bind(
+      &AssistedTeleop::preemptTeleopCallback,
+      this, std::placeholders::_1));
+}
+
+Status AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAction::Goal> command)
+{
+  preempt_teleop_ = false;
+  command_time_allowance_ = command->time_allowance;
+  end_time_ = this->clock_->now() + command_time_allowance_;
+  return Status::SUCCEEDED;
+}
+
+void AssistedTeleop::onActionCompletion()
+{
+  teleop_twist_ = geometry_msgs::msg::Twist();
+  preempt_teleop_ = false;
+}
+
+Status AssistedTeleop::onCycleUpdate()
+{
+  feedback_->current_teleop_duration = elasped_time_;
+  action_server_->publish_feedback(feedback_);
+
+  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
+  if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
+    stopRobot();
+    RCLCPP_WARN_STREAM(
+      logger_,
+      "Exceeded time allowance before reaching the " << behavior_name_.c_str() <<
+        "goal - Exiting " << behavior_name_.c_str());
+    return Status::FAILED;
+  }
+
+  // user states that teleop was successful
+  if (preempt_teleop_) {
+    stopRobot();
+    return Status::SUCCEEDED;
+  }
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR_STREAM(
+      logger_,
+      "Current robot pose is not available for " <<
+        behavior_name_.c_str());
+    return Status::FAILED;
+  }
+  geometry_msgs::msg::Pose2D projected_pose;
+  projected_pose.x = current_pose.pose.position.x;
+  projected_pose.y = current_pose.pose.position.y;
+  projected_pose.theta = tf2::getYaw(current_pose.pose.orientation);
+
+  geometry_msgs::msg::Twist scaled_twist = teleop_twist_;
+  for (double time = simulation_time_step_; time < projection_time_;
+    time += simulation_time_step_)
+  {
+    projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_);
+
+    if (!collision_checker_->isCollisionFree(projected_pose)) {
+      if (time == simulation_time_step_) {
+        RCLCPP_DEBUG_STREAM_THROTTLE(
+          logger_,
+          *clock_,
+          1000,
+          behavior_name_.c_str() << " collided on first time step, setting velocity to zero");
+        scaled_twist.linear.x = 0.0f;
+        scaled_twist.linear.y = 0.0f;
+        scaled_twist.angular.z = 0.0f;
+        break;
+      } else {
+        RCLCPP_DEBUG_STREAM_THROTTLE(
+          logger_,
+          *clock_,
+          1000,
+          behavior_name_.c_str() << " collision approaching in " << time << " seconds");
+        double scale_factor = time / projection_time_;
+        scaled_twist.linear.x *= scale_factor;
+        scaled_twist.linear.y *= scale_factor;
+        scaled_twist.angular.z *= scale_factor;
+        break;
+      }
+    }
+  }
+  vel_pub_->publish(std::move(scaled_twist));
+
+  return Status::RUNNING;
+}
+
+geometry_msgs::msg::Pose2D AssistedTeleop::projectPose(
+  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Twist & twist,
+  double projection_time)
+{
+  geometry_msgs::msg::Pose2D projected_pose = pose;
+
+  projected_pose.x += projection_time * (
+    twist.linear.x * cos(pose.theta) +
+    twist.linear.y * sin(pose.theta));
+
+  projected_pose.y += projection_time * (
+    twist.linear.x * sin(pose.theta) -
+    twist.linear.y * cos(pose.theta));
+
+  projected_pose.theta += projection_time * twist.angular.z;
+
+  return projected_pose;
+}
+
+void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  teleop_twist_ = *msg;
+}
+
+void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr)
+{
+  preempt_teleop_ = true;
+}
+
+}  // namespace nav2_behaviors
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::AssistedTeleop, nav2_core::Behavior)

navigation2/nav2_behaviors/plugins/back_up.cpp

@@ -0,0 +1,50 @@
+// 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.
+
+#include "nav2_behaviors/plugins/back_up.hpp"
+
+namespace nav2_behaviors
+{
+
+Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
+{
+  if (command->target.y != 0.0 || command->target.z != 0.0) {
+    RCLCPP_INFO(
+      logger_,
+      "Backing up in Y and Z not supported, will only move in X.");
+    return Status::FAILED;
+  }
+
+  // Silently ensure that both the speed and direction are negative.
+  command_x_ = -std::fabs(command->target.x);
+  command_speed_ = -std::fabs(command->speed);
+  command_time_allowance_ = command->time_allowance;
+
+  end_time_ = this->clock_->now() + command_time_allowance_;
+
+  if (!nav2_util::getCurrentPose(
+      initial_pose_, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR(logger_, "Initial robot pose is not available.");
+    return Status::FAILED;
+  }
+
+  return Status::SUCCEEDED;
+}
+
+}  // namespace nav2_behaviors
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::BackUp, nav2_core::Behavior)

navigation2/nav2_behaviors/plugins/drive_on_heading.cpp

@@ -0,0 +1,20 @@
+// 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.
+
+#include <memory>
+#include "nav2_behaviors/plugins/drive_on_heading.hpp"
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior)

navigation2/nav2_behaviors/plugins/spin.cpp

@@ -0,0 +1,192 @@
+// 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 "tf2/utils.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/node_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"};
+  }
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "simulate_ahead_time", rclcpp::ParameterValue(2.0));
+  node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "max_rotational_vel", rclcpp::ParameterValue(1.0));
+  node->get_parameter("max_rotational_vel", max_rotational_vel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "min_rotational_vel", rclcpp::ParameterValue(0.4));
+  node->get_parameter("min_rotational_vel", min_rotational_vel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "rotational_acc_lim", rclcpp::ParameterValue(3.2));
+  node->get_parameter("rotational_acc_lim", rotational_acc_lim_);
+}
+
+Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
+{
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR(logger_, "Current robot pose is not available.");
+    return Status::FAILED;
+  }
+
+  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;
+  end_time_ = this->clock_->now() + command_time_allowance_;
+
+  return Status::SUCCEEDED;
+}
+
+Status Spin::onCycleUpdate()
+{
+  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
+  if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
+    stopRobot();
+    RCLCPP_WARN(
+      logger_,
+      "Exceeded time allowance before reaching the Spin goal - Exiting Spin");
+    return Status::FAILED;
+  }
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR(logger_, "Current robot pose is not available.");
+    return Status::FAILED;
+  }
+
+  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 Status::SUCCEEDED;
+  }
+
+  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::Twist>();
+  cmd_vel->angular.z = copysign(vel, cmd_yaw_);
+
+  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(relative_yaw_, cmd_vel.get(), pose2d)) {
+    stopRobot();
+    RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
+    return Status::FAILED;
+  }
+
+  vel_pub_->publish(std::move(cmd_vel));
+
+  return Status::RUNNING;
+}
+
+bool Spin::isCollisionFree(
+  const double & relative_yaw,
+  geometry_msgs::msg::Twist * cmd_vel,
+  geometry_msgs::msg::Pose2D & pose2d)
+{
+  // 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::Pose2D init_pose = pose2d;
+  bool fetch_data = true;
+
+  while (cycle_count < max_cycle_count) {
+    sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_);
+    pose2d.theta = init_pose.theta + sim_position_change;
+    cycle_count++;
+
+    if (abs(relative_yaw) - abs(sim_position_change) <= 0.) {
+      break;
+    }
+
+    if (!collision_checker_->isCollisionFree(pose2d, 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)

navigation2/nav2_behaviors/plugins/wait.cpp

@@ -0,0 +1,55 @@
+// Copyright (c) 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 <chrono>
+#include <memory>
+
+#include "nav2_behaviors/plugins/wait.hpp"
+
+namespace nav2_behaviors
+{
+
+Wait::Wait()
+: TimedBehavior<WaitAction>(),
+  feedback_(std::make_shared<WaitAction::Feedback>())
+{
+}
+
+Wait::~Wait() = default;
+
+Status Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
+{
+  wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time);
+  return Status::SUCCEEDED;
+}
+
+Status Wait::onCycleUpdate()
+{
+  auto current_point = node_.lock()->now();
+  auto time_left = wait_end_ - current_point;
+
+  feedback_->time_left = time_left;
+  action_server_->publish_feedback(feedback_);
+
+  if (time_left.nanoseconds() > 0) {
+    return Status::RUNNING;
+  } else {
+    return Status::SUCCEEDED;
+  }
+}
+
+}  // namespace nav2_behaviors
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Wait, nav2_core::Behavior)