Merge remote-tracking branch 'upstream/gpsr-state-machine-factory' in…

…to ben_gpsr

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "common/third_party/leg_tracker"]
 	path = common/third_party/leg_tracker
-	url = git@github.com:angusleigh/leg_tracker.git
+	url = https://github.com/LASR-at-Home/leg_tracker

common/helpers/navigation_helpers/src/navigation_helpers/__init__.py

@@ -1,9 +1,18 @@
+import rospy
+
+
 from geometry_msgs.msg import (
     Point,
     Pose,
+    PoseStamped,
+    Quaternion,
 )
+from nav_msgs.srv import GetPlan
+from nav_msgs.msg import Path
 
 import numpy as np
+import math
+from scipy.spatial.transform import Rotation as R
 from itertools import permutations
 
 from typing import Union, List
@@ -27,3 +36,49 @@ def min_hamiltonian_path(start: Pose, poses: List[Pose]) -> Union[None, List[Pos
             best_order = list(perm)
 
     return best_order
+
+
+def get_pose_on_path(
+    p1: PoseStamped, p2: PoseStamped, dist_to_goal: float = 1.0, tolerance: float = 0.5
+) -> Union[None, PoseStamped]:
+    make_plan: rospy.ServiceProxy = rospy.ServiceProxy("/move_base/make_plan", GetPlan)
+
+    chosen_pose: Union[None, PoseStamped] = None
+
+    rospy.loginfo(f"Getting plan from {p1} to {p2}.")
+
+    if p1.header.frame_id != p2.header.frame_id != "map":
+        rospy.loginfo(
+            f"Frames of reference are not 'map' ({p1.header.frame_id} and {p2.header.frame_id})."
+        )
+        return chosen_pose
+
+    try:
+        make_plan.wait_for_service(timeout=rospy.Duration.from_sec(10.0))
+    except rospy.ROSException:
+        rospy.loginfo("Service /move_base/make_plan not available.")
+        return chosen_pose
+
+    try:
+        plan: Path = make_plan(p1, p2, tolerance).plan
+    except rospy.ServiceException as e:
+        rospy.loginfo(e)
+        return chosen_pose
+
+    rospy.loginfo(f"Got plan with {len(plan.poses)} poses.")
+
+    if len(plan.poses) > 0:
+        for pose in reversed(plan.poses):
+            if euclidian_distance(pose.pose.position, p2.pose.position) >= dist_to_goal:
+                chosen_pose = pose
+                break
+
+    return chosen_pose
+
+
+def compute_face_quat(p1: Pose, p2: Pose) -> Quaternion:
+    dx: float = p2.position.x - p1.position.x
+    dy: float = p2.position.y - p1.position.y
+    theta_deg = np.degrees(math.atan2(dy, dx))
+    x, y, z, w = R.from_euler("z", theta_deg, degrees=True).as_quat()
+    return Quaternion(x, y, z, w)