Created path segmenter controller

toid_spinner_controller/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.8)
+project(toid_spinner_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(library_name toid_spinner_controller)
+
+
+set(PACKAGE_DEPS
+  rclcpp
+  geometry_msgs
+  nav2_costmap_2d
+  pluginlib
+  nav_msgs
+  nav2_util
+  nav2_core
+  tf2
+  tf2_ros
+  angles
+  nav2_controller
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach(PACKAGE ${PACKAGE_DEPS})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+add_library(${library_name} SHARED
+        src/toid_spinner_controller.cpp
+        src/path_handler.cpp)
+
+target_include_directories(${library_name}
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(${library_name}
+  ${PACKAGE_DEPS}
+)
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${PACKAGE_DEPS})
+
+pluginlib_export_plugin_description_file(nav2_core toid_spinner_controller.xml)
+
+ament_package()

toid_spinner_controller/LICENSE

@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <memory>
+#include <rclcpp/logger.hpp>
+#include <tf2_ros/buffer.hpp>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "tf2/time.hpp"
+
+namespace toid {
+
+class PathHandler {
+public:
+  PathHandler(tf2::Duration tranform_tolerance,
+              std::shared_ptr<tf2_ros::Buffer> tf,
+              std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
+
+  ~PathHandler() = default;
+
+  nav_msgs::msg::Path
+  transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose,
+                      double max_robot_pose_search_dist,
+                      double angle_tolerance);
+
+  bool tranformPose(const std::string frame,
+                    const geometry_msgs::msg::PoseStamped &in_pose,
+                    geometry_msgs::msg::PoseStamped &out_pose);
+
+  void setPlan(const nav_msgs::msg::Path &path) { global_plan_ = path; }
+
+  nav_msgs::msg::Path getPlan() { return global_plan_; }
+
+protected:
+
+  double dist2(const geometry_msgs::msg::PoseStamped &p1, const geometry_msgs::msg::PoseStamped &p2);
+
+  rclcpp::Logger logger_{rclcpp::get_logger("MultiPlexPathHandler")};
+  tf2::Duration transform_tolerance_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav_msgs::msg::Path global_plan_;
+};
+
+} // namespace toid

toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp

@@ -0,0 +1,82 @@
+#pragma once
+#include <memory>
+
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav2_controller/plugins/position_goal_checker.hpp"
+#include "nav2_core/controller.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "toid_spinner_controller/path_handler.hpp"
+
+namespace toid {
+class SpinnerController : public nav2_core::Controller {
+public:
+  SpinnerController();
+  ~SpinnerController() override = default;
+
+  void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
+                 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+                 std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) override;
+
+  void cleanup() override;
+
+  void activate() override;
+
+  void deactivate() override;
+
+  void setPlan(const nav_msgs::msg::Path &path) override;
+
+  bool cancel() override;
+
+  geometry_msgs::msg::TwistStamped
+  computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
+                          const geometry_msgs::msg::Twist &velocity,
+                          nav2_core::GoalChecker *goal_checker) override;
+
+  geometry_msgs::msg::TwistStamped
+  computeRotateToHeadingCommand(const double &angular_distance_to_heading,
+                                const geometry_msgs::msg::PoseStamped &pose);
+
+  void setSpeedLimit(const double &speed_limit,
+                     const bool &percentage) override;
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+  rclcpp::Logger logger_{rclcpp::get_logger("SpinnerController")};
+  rclcpp::Clock::SharedPtr clock_;
+  std::unique_ptr<PathHandler> path_handler_;
+  std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
+  std::unique_ptr<
+      nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
+      collision_checker_;
+  rclcpp_lifecycle::LifecyclePublisher<
+      geometry_msgs::msg::PointStamped>::SharedPtr closest_pt_pub_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  bool new_path_recieved = false;
+  double last_angular_vel_;
+
+  pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
+  nav2_core::Controller::Ptr primary_controller_;
+
+  // Params
+  double angular_dist_threshold_;
+  double angular_disengage_threshold_;
+  double forward_sampling_distance_;
+  double max_angular_accel_;
+  double rotate_to_heading_;
+
+  double control_duration_;
+};
+} // namespace toid

toid_spinner_controller/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_spinner_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">petar</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>angles</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_core</depend>
+  <depend>nav2_controller</depend>
+  <depend>pluginlib</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/toid_spinner_controller.xml" />
+  </export>
+</package>

toid_spinner_controller/src/path_handler.cpp

@@ -0,0 +1,131 @@
+#include "toid_spinner_controller/path_handler.hpp"
+#include "angles/angles.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2/LinearMath/Vector3.hpp"
+#include "tf2/exceptions.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <iterator>
+#include <memory>
+#include <numeric>
+
+namespace toid {
+PathHandler::PathHandler(
+    tf2::Duration tranform_tolerance, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+    : transform_tolerance_(tranform_tolerance), tf_(tf),
+      costmap_ros_(costmap_ros) {}
+
+nav_msgs::msg::Path
+PathHandler::transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose,
+                                 double max_robot_pose_search_dist,
+                                 double angle_tolerance) {
+
+  if (global_plan_.poses.empty()) {
+    throw nav2_core::InvalidPath("Received plan with length of one");
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!tranformPose(global_plan_.header.frame_id, pose, robot_pose)) {
+    throw nav2_core::ControllerTFError(
+        "Unable to transform robot pose into global plan's frame");
+  }
+
+  auto closest_pose_upper_bound =
+      nav2_util::geometry_utils::first_after_integrated_distance(
+          global_plan_.poses.begin(), global_plan_.poses.end(),
+          max_robot_pose_search_dist);
+
+  const double robot_orientation = tf2::getYaw(robot_pose.pose.orientation);
+
+  // Calculate the closeset pose and keep track of the next rotation point
+  auto closest_pose = global_plan_.poses.begin();
+  auto last_rotation = global_plan_.poses.end();
+  auto previous_it = global_plan_.poses.begin();
+  auto it = global_plan_.poses.begin();
+  double closest_dist = INFINITY;
+
+  for (; it != closest_pose_upper_bound; it++) {
+    const double it_orientation = tf2::getYaw(it->pose.orientation);
+    const double angle_diff = std::abs(
+        angles::shortest_angular_distance(it_orientation, robot_orientation));
+
+    if (previous_it->pose.position == it->pose.position) {
+      last_rotation = it + 1;
+    }
+
+    if (angle_diff <= angle_tolerance) {
+      double dist = dist2(robot_pose, *it);
+      if (dist <= closest_dist) {
+        closest_dist = dist;
+        closest_pose = it;
+        last_rotation = global_plan_.poses.end();
+      }
+    }
+    previous_it = it;
+  }
+  if(last_rotation == closest_pose_upper_bound || last_rotation == global_plan_.poses.end()) {
+    for (; it != global_plan_.poses.end(); it++) {
+      if (previous_it->pose.position == it->pose.position) {
+        last_rotation = it + 1;
+      } else if (last_rotation != global_plan_.poses.end()) {
+        break;
+      }
+      previous_it = it;
+    }
+  }
+
+  auto transformGlobalPoseToLocal = [&](const auto &global_plan_pose) {
+    geometry_msgs::msg::PoseStamped stamped_pose;
+    stamped_pose.header.frame_id = global_plan_.header.frame_id;
+    stamped_pose.header.stamp = robot_pose.header.stamp;
+    stamped_pose.pose = global_plan_pose.pose;
+    stamped_pose.pose.position.z = 0.0;
+    return stamped_pose;
+  };
+
+  nav_msgs::msg::Path out_path;
+  std::transform(closest_pose, last_rotation,
+                 std::back_inserter(out_path.poses),
+                 transformGlobalPoseToLocal);
+
+  out_path.header.frame_id = global_plan_.header.frame_id;
+  out_path.header.stamp = robot_pose.header.stamp;
+
+  global_plan_.poses.erase(begin(global_plan_.poses), closest_pose);
+
+  return out_path;
+}
+
+bool PathHandler::tranformPose(const std::string frame,
+                               const geometry_msgs::msg::PoseStamped &in_pose,
+                               geometry_msgs::msg::PoseStamped &out_pose) {
+  if (in_pose.header.frame_id == frame) {
+    out_pose = in_pose;
+    return true;
+  }
+
+  try {
+    tf_->transform(in_pose, out_pose, frame, transform_tolerance_);
+    out_pose.header.frame_id = frame;
+    return true;
+  } catch (tf2::TransformException &ex) {
+    RCLCPP_ERROR(logger_, "Exception in tranformPose: %s", ex.what());
+  }
+  return false;
+}
+
+double PathHandler::dist2(const geometry_msgs::msg::PoseStamped &p1,
+                          const geometry_msgs::msg::PoseStamped &p2) {
+  const double x = p1.pose.position.x - p2.pose.position.x;
+  const double y = p1.pose.position.y - p2.pose.position.y;
+  return x * x + y * y;
+}
+
+} // namespace toid

toid_spinner_controller/src/toid_spinner_controller.cpp

@@ -0,0 +1,206 @@
+#include "toid_spinner_controller/toid_spinner_controller.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "pluginlib/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "toid_spinner_controller/path_handler.hpp"
+
+#include <algorithm>
+#include <angles/angles.h>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <rclcpp/duration.hpp>
+#include <tf2/time.hpp>
+#include <tf2/utils.hpp>
+
+namespace toid {
+SpinnerController::SpinnerController()
+    : lp_loader_("nav2_core", "nav2_core::Controller") {}
+
+void SpinnerController::configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
+    std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
+
+  position_goal_checker_ =
+      std::make_unique<nav2_controller::PositionGoalChecker>();
+  position_goal_checker_->initialize(parent, ".position_checker", costmap_ros);
+  plugin_name_ = name;
+  node_ = parent;
+  auto node = node_.lock();
+
+  tf_ = tf;
+  logger_ = node->get_logger();
+  clock_ = node->get_clock();
+
+  std::string primary_controller;
+  double control_frequency;
+
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".angular_dist_threshold",
+      rclcpp::ParameterValue(0.785));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".angular_disengage_threshold",
+      rclcpp::ParameterValue(0.785 / 2.0));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".forward_sampling_distance",
+      rclcpp::ParameterValue(0.5));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".rotate_to_heading", rclcpp::ParameterValue(1.0));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".primary_controller.plugin",
+      rclcpp::PARAMETER_STRING);
+
+  node->get_parameter(plugin_name_ + ".angular_dist_threshold",
+                      angular_dist_threshold_);
+  node->get_parameter(plugin_name_ + ".angular_disengage_threshold",
+                      angular_disengage_threshold_);
+  node->get_parameter(plugin_name_ + ".forward_sampling_distance",
+                      forward_sampling_distance_);
+  node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
+  node->get_parameter(plugin_name_ + ".rotate_to_heading", rotate_to_heading_);
+
+  primary_controller =
+      node->get_parameter(plugin_name_ + ".primary_controller.plugin")
+          .as_string();
+
+  node->get_parameter("controller_frequency", control_frequency);
+  control_duration_ = 1.0 / control_frequency;
+
+  try {
+    primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
+    RCLCPP_INFO(logger_,
+                "Created internal controller for multiplexer: %s of type %s",
+                plugin_name_.c_str(), primary_controller.c_str());
+  } catch (const pluginlib::PluginlibException &ex) {
+    RCLCPP_FATAL(
+        logger_,
+        "Failed to create primary controller for multiplexer. Exception: %s",
+        ex.what());
+    return;
+  }
+  primary_controller_->configure(parent, name + ".primary_controller", tf,
+                                 costmap_ros);
+
+  path_handler_ = std::make_unique<PathHandler>(tf2::durationFromSec(0.1), tf_,
+                                                costmap_ros);
+
+  closest_pt_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
+      "start_point", 1);
+}
+
+void SpinnerController::cleanup() {
+  RCLCPP_INFO(logger_, "Cleaning SpinnerController");
+  primary_controller_->cleanup();
+  primary_controller_->reset();
+  position_goal_checker_.reset();
+  path_handler_.reset();
+  closest_pt_pub_.reset();
+}
+
+void SpinnerController::activate() {
+  RCLCPP_INFO(logger_, "Activating SpinnerController");
+  last_angular_vel_ = std::numeric_limits<double>::max();
+  new_path_recieved = false;
+  auto node = node_.lock();
+  closest_pt_pub_->on_activate();
+  primary_controller_->activate();
+  position_goal_checker_->reset();
+}
+
+void SpinnerController::deactivate() {
+  RCLCPP_INFO(logger_, "Deactivating SpinnerController");
+  closest_pt_pub_.reset();
+  primary_controller_->deactivate();
+  closest_pt_pub_->on_deactivate();
+}
+
+void SpinnerController::setPlan(const nav_msgs::msg::Path &path) {
+  path_handler_->setPlan(path);
+  new_path_recieved = true;
+  primary_controller_->setPlan(path);
+}
+
+geometry_msgs::msg::TwistStamped SpinnerController::computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped &pose,
+    const geometry_msgs::msg::Twist &velocity,
+    nav2_core::GoalChecker *goal_checker) {
+
+  auto path = path_handler_->transformGlobalPlan(pose, 1.0, M_PI / 8);
+
+  if (path.poses.size() > 0) {
+    geometry_msgs::msg::PointStamped pt;
+    const auto &pose = path.poses.back();
+    pt.point = pose.pose.position;
+    pt.header.frame_id = path.header.frame_id;
+    pt.header.stamp = path.header.stamp;
+    closest_pt_pub_->publish(pt);
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  path_handler_->tranformPose(path.header.frame_id, pose, robot_pose);
+  const double robot_angle = tf2::getYaw(robot_pose.pose.orientation);
+
+  if (path.poses.front().pose.position == path.poses.back().pose.position) {
+    const double path_angle = tf2::getYaw(path.poses.back().pose.orientation);
+    const double angle =
+        angles::shortest_angular_distance(robot_angle, path_angle);
+    return computeRotateToHeadingCommand(angle, pose);
+  }
+
+  if (new_path_recieved) {
+    new_path_recieved = false;
+    primary_controller_->setPlan(path);
+  }
+
+  auto twist = primary_controller_->computeVelocityCommands(pose, velocity,
+                                                            goal_checker);
+  last_angular_vel_ = twist.twist.angular.z;
+  return twist;
+}
+
+geometry_msgs::msg::TwistStamped
+SpinnerController::computeRotateToHeadingCommand(
+    const double &angular_distance_to_heading,
+    const geometry_msgs::msg::PoseStamped &pose) {
+  geometry_msgs::msg::TwistStamped cmd;
+
+  const double last_angular_vel =
+      (last_angular_vel_ == std::numeric_limits<double>::max())
+          ? 0
+          : last_angular_vel_;
+  const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
+  const double speed_upper_bound =
+      last_angular_vel + control_duration_ * max_angular_accel_;
+  const double speed_lower_bound =
+      last_angular_vel - control_duration_ * max_angular_accel_;
+
+  const double speed_until_overshoot =
+      0.9 * std::sqrt(2.0 * max_angular_accel_ *
+                    std::fabs(angular_distance_to_heading));
+  const double requested_speed = sign * std::fmin(speed_until_overshoot,rotate_to_heading_);
+  const double speed =
+      std::clamp(requested_speed, speed_lower_bound, speed_upper_bound);
+
+  cmd.header.frame_id = pose.header.frame_id;
+  cmd.header.stamp = pose.header.stamp;
+  cmd.twist.angular.z = speed;
+  last_angular_vel_ = speed;
+  return cmd;
+}
+
+void SpinnerController::setSpeedLimit(const double &speed_limit,
+                                      const bool &percentage) {
+  primary_controller_->setSpeedLimit(speed_limit, percentage);
+}
+
+bool SpinnerController::cancel() { return primary_controller_->cancel(); }
+
+} // namespace toid
+
+PLUGINLIB_EXPORT_CLASS(toid::SpinnerController, nav2_core::Controller)

toid_spinner_controller/toid_spinner_controller.xml

@@ -0,0 +1,9 @@
+<class_libraries>
+    <library path="toid_spinner_controller">
+        <class type="toid::SpinnerController" base_class_type="nav2_core::Controller">
+            <description>
+                toid_SpinnerController
+            </description>
+        </class>
+    </library>
+</class_libraries>