Created path segmenter controller

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