geometry utils

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -188,6 +188,51 @@ inline int find_next_matching_goal_in_waypoint_statuses(
   return itr - waypoint_statuses.begin();
 }
 
+/**
+ * @brief Find the distance to a point
+ * @param global_pose Robot's current or planned position
+ * @param target
+ * @return int
+ */
+inline double distanceToPoint(const geometry_msgs::msg::PoseStamped &global_pose,
+                                const geometry_msgs::msg::PoseStamped &target)
+{
+    const double xDist = global_pose.pose.position.x - target.pose.position.x;
+    const double yDist = global_pose.pose.position.y - target.pose.position.y;
+    return std::hypot(xDist,yDist);
+}
+
+/**
+ * @brief Find the shortest distance to a vector
+ * @param global_pose Robot's current or planned position
+ * @param start Starting point of target vector
+ * @param finish End point of target vector
+ * @return int
+ */
+inline double distanceToSegment(
+const geometry_msgs::msg::PoseStamped & point,
+const geometry_msgs::msg::PoseStamped & start,
+const geometry_msgs::msg::PoseStamped & end)
+{
+const auto & p = point.pose.position;
+const auto & a = start.pose.position;
+const auto & b = end.pose.position;
+
+const double seg_len_sq = distanceToPoint(start, end);
+if (seg_len_sq <= 1e-8) {
+    return std::sqrt(distanceToPoint(start, end));
+}
+
+const double dot = ((p.x - a.x) * (b.x - a.x)) + ((p.y - a.y) * (b.y - a.y));
+const double t = std::clamp(dot / seg_len_sq, 0.0, 1.0);
+
+const double proj_x = a.x + t * (b.x - a.x);
+const double proj_y = a.y + t * (b.y - a.y);
+
+const double dx_proj = p.x - proj_x;
+const double dy_proj = p.y - proj_y;
+return std::hypot(dx_proj,dy_proj);
+}
 }  // namespace geometry_utils
 }  // namespace nav2_util