Move from controller_utils to path_utils

Signed-off-by: mini-1235 <[email protected]>

Author: mini-1235

nav2_controller/plugins/simple_path_handler.cpp

@@ -18,7 +18,7 @@
 #include "angles/angles.h"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_controller/plugins/simple_path_handler.hpp"
-#include "nav2_util/controller_utils.hpp"
+#include "nav2_util/path_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_core/controller_exceptions.hpp"
 

nav2_util/include/nav2_util/controller_utils.hpp

@@ -62,73 +62,6 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint(
   double &, const nav_msgs::msg::Path &,
   const bool interpolate_after_goal = false);
 
-/**
- * @brief Find the iterator of the first pose at which there is an inversion on the path,
- * @param path to check for inversion
- * @return the first point after the inversion found in the path
- */
-inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
-{
-  // At least 3 poses for a possible inversion
-  if (path.poses.size() < 3) {
-    return path.poses.size();
-  }
-
-  // Iterating through the path to determine the position of the path inversion
-  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
-    // We have two vectors for the dot product OA and AB. Determining the vectors.
-    float oa_x = path.poses[idx].pose.position.x -
-      path.poses[idx - 1].pose.position.x;
-    float oa_y = path.poses[idx].pose.position.y -
-      path.poses[idx - 1].pose.position.y;
-    float ab_x = path.poses[idx + 1].pose.position.x -
-      path.poses[idx].pose.position.x;
-    float ab_y = path.poses[idx + 1].pose.position.y -
-      path.poses[idx].pose.position.y;
-
-    // Checking for the existence of cusp, in the path, using the dot product.
-    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
-    if (dot_product < 0.0f) {
-      return idx + 1;
-    }
-
-    if (
-      (hypot(oa_x, oa_y) == 0.0 &&
-      path.poses[idx - 1].pose.orientation !=
-      path.poses[idx].pose.orientation)
-      ||
-      (hypot(ab_x, ab_y) == 0.0 &&
-      path.poses[idx].pose.orientation !=
-      path.poses[idx + 1].pose.orientation))
-    {
-      // returning the distance since the points overlap
-      // but are not simply duplicate points (e.g. in place rotation)
-      return idx + 1;
-    }
-  }
-
-  return path.poses.size();
-}
-
-/**
- * @brief Find and remove poses after the first inversion in the path
- * @param path to check for inversion
- * @return The location of the inversion, return 0 if none exist
- */
-inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
-{
-  nav_msgs::msg::Path cropped_path = path;
-  const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
-  if (first_after_inversion == path.poses.size()) {
-    return 0u;
-  }
-
-  cropped_path.poses.erase(
-    cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
-  path = cropped_path;
-  return first_after_inversion;
-}
-
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__CONTROLLER_UTILS_HPP_

nav2_util/include/nav2_util/path_utils.hpp

@@ -74,6 +74,73 @@ bool transformPathInTargetFrame(
   tf2_ros::Buffer & tf_buffer, const std::string target_frame,
   const double transform_timeout = 0.1);
 
+/**
+ * @brief Find the iterator of the first pose at which there is an inversion on the path,
+ * @param path to check for inversion
+ * @return the first point after the inversion found in the path
+ */
+inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
+{
+  // At least 3 poses for a possible inversion
+  if (path.poses.size() < 3) {
+    return path.poses.size();
+  }
+
+  // Iterating through the path to determine the position of the path inversion
+  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+    // We have two vectors for the dot product OA and AB. Determining the vectors.
+    float oa_x = path.poses[idx].pose.position.x -
+      path.poses[idx - 1].pose.position.x;
+    float oa_y = path.poses[idx].pose.position.y -
+      path.poses[idx - 1].pose.position.y;
+    float ab_x = path.poses[idx + 1].pose.position.x -
+      path.poses[idx].pose.position.x;
+    float ab_y = path.poses[idx + 1].pose.position.y -
+      path.poses[idx].pose.position.y;
+
+    // Checking for the existence of cusp, in the path, using the dot product.
+    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+    if (dot_product < 0.0f) {
+      return idx + 1;
+    }
+
+    if (
+      (hypot(oa_x, oa_y) == 0.0 &&
+      path.poses[idx - 1].pose.orientation !=
+      path.poses[idx].pose.orientation)
+      ||
+      (hypot(ab_x, ab_y) == 0.0 &&
+      path.poses[idx].pose.orientation !=
+      path.poses[idx + 1].pose.orientation))
+    {
+      // returning the distance since the points overlap
+      // but are not simply duplicate points (e.g. in place rotation)
+      return idx + 1;
+    }
+  }
+
+  return path.poses.size();
+}
+
+/**
+ * @brief Find and remove poses after the first inversion in the path
+ * @param path to check for inversion
+ * @return The location of the inversion, return 0 if none exist
+ */
+inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
+{
+  nav_msgs::msg::Path cropped_path = path;
+  const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
+  if (first_after_inversion == path.poses.size()) {
+    return 0u;
+  }
+
+  cropped_path.poses.erase(
+    cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
+  path = cropped_path;
+  return first_after_inversion;
+}
+
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__PATH_UTILS_HPP_

nav2_util/test/test_controller_utils.cpp

@@ -387,66 +387,3 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   EXPECT_NEAR(pt.pose.position.x, 2.0 + cos(-45.0 * M_PI / 180) * 10.0, EPSILON);
   EXPECT_NEAR(pt.pose.position.y, -2.0 + sin(-45.0 * M_PI / 180) * 10.0, EPSILON);
 }
-
-TEST(UtilsTests, FindPathInversionTest)
-{
-  // Straight path, no inversions to be found
-  nav_msgs::msg::Path path;
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u);
-
-  // To short to process
-  path.poses.erase(path.poses.begin(), path.poses.begin() + 7);
-  EXPECT_EQ(nav2_util::findFirstPathInversion(path), 3u);
-
-  // Has inversion at index 10, so should return 11 for the first point afterwards
-  // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1
-  path.poses.clear();
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = 10 - i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(nav2_util::findFirstPathInversion(path), 11u);
-}
-
-TEST(UtilsTests, RemovePosesAfterPathInversionTest)
-{
-  nav_msgs::msg::Path path;
-  // straight path
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u);
-
-  // try empty path
-  path.poses.clear();
-  EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u);
-
-  // cusping path
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = i;
-    path.poses.push_back(pose);
-  }
-  for (unsigned int i = 0; i != 10; i++) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.pose.position.x = 10 - i;
-    path.poses.push_back(pose);
-  }
-  EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 11u);
-  // Check to see if removed
-  EXPECT_EQ(path.poses.size(), 11u);
-  EXPECT_EQ(path.poses.back().pose.position.x, 10);
-}

nav2_util/test/test_path_utils.cpp

@@ -456,3 +456,86 @@ TEST(TransformPathTest, MissingTransform)
     input_path, transformed_path, *tf_buffer_, "base_link", 0.1));
   rclcpp::shutdown();
 }
+
+TEST(UtilsTests, FindPathInversionTest)
+{
+  // Straight path, no inversions to be found
+  nav_msgs::msg::Path path;
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u);
+
+  // To short to process
+  path.poses.erase(path.poses.begin(), path.poses.begin() + 7);
+  EXPECT_EQ(nav2_util::findFirstPathInversion(path), 3u);
+
+  // Has inversion at index 10, so should return 11 for the first point afterwards
+  // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1
+  path.poses.clear();
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = 10 - i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(nav2_util::findFirstPathInversion(path), 11u);
+
+  // In place rotation
+  path.poses.clear();
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  geometry_msgs::msg::PoseStamped last_pose;
+  last_pose = path.poses.back();
+  tf2::Quaternion q;
+  q.setRPY(0, 0, M_PI_2);  // rotate 90 degrees
+  last_pose.pose.orientation.x = q.x();
+  last_pose.pose.orientation.y = q.y();
+  last_pose.pose.orientation.z = q.z();
+  last_pose.pose.orientation.w = q.w();
+  path.poses.push_back(last_pose);
+  last_pose.pose.position.x = 11.0;
+  path.poses.push_back(last_pose);
+  EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u);
+}
+
+TEST(UtilsTests, RemovePosesAfterPathInversionTest)
+{
+  nav_msgs::msg::Path path;
+  // straight path
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u);
+
+  // try empty path
+  path.poses.clear();
+  EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u);
+
+  // cusping path
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = 10 - i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 11u);
+  // Check to see if removed
+  EXPECT_EQ(path.poses.size(), 11u);
+  EXPECT_EQ(path.poses.back().pose.position.x, 10);
+}