Vs/approach spin

config/navigation.yaml

@@ -71,6 +71,8 @@ navigation:
       safe_approach_distance: 5.0
       angle_thresh: 0.0872665 #pi/36 radians / 5 degrees
       distance_threshold: 1.0
+      distance_look_threshold: 5.0
+      check_update_time: 0.06 #seconds
       update_delay: 3.0
 
     single_tag:

navigation/approach_target.py

@@ -1,14 +1,11 @@
 import numpy as np
-from typing import Any
 from navigation.trajectory import Trajectory
 from navigation.astar import AStar, NoPath, OutOfBounds
 from . import costmap_search, stuck_recovery, waypoint, backup, state
 from .context import Context
 from state_machine.state import State
 from geometry_msgs.msg import Twist
 from nav_msgs.msg import Path
-from visualization_msgs.msg import Marker
-from rclpy.publisher import Publisher
 from rclpy.time import Time
 from rclpy.timer import Timer
 from rclpy.duration import Duration
@@ -19,6 +16,8 @@ class ApproachTargetState(State):
     UPDATE_DELAY: float
     USE_COSTMAP: bool
     DISTANCE_THRESHOLD: float
+    LOOK_DISTANCE_THRESHOLD: float
+    CHECK_UPDATE_TIME: float
     time_begin: Time
     astar_traj: Trajectory
     target_traj: Trajectory
@@ -27,6 +26,7 @@ class ApproachTargetState(State):
     target_position: np.ndarray | None
     marker_timer: Timer
     update_timer: Timer
+    looking_for_object: bool
 
     def on_enter(self, context: Context) -> None:
         from .long_range import LongRangeState
@@ -46,12 +46,15 @@ def on_enter(self, context: Context) -> None:
 
         self.USE_COSTMAP = context.node.get_parameter("costmap.use_costmap").value or current_waypoint.enable_costmap
         self.DISTANCE_THRESHOLD = context.node.get_parameter("search.distance_threshold").value
+        self.LOOK_DISTANCE_THRESHOLD = context.node.get_parameter("search.distance_look_threshold").value
+        self.CHECK_UPDATE_TIME = context.node.get_parameter("search.check_update_time").value
         self.astar_traj = Trajectory(np.array([]))
         self.target_traj = Trajectory(np.array([]))
         self.astar = AStar(context=context)
         self.target_position = None
         self.time_last_updated = context.node.get_clock().now()
         self.time_begin = context.node.get_clock().now()
+        self.looking_for_object = context.course.look_for_object()
 
         self.marker_timer = context.node.create_timer(
             context.node.get_parameter("pub_path_rate").value, lambda: self.display_markers(context=context)
@@ -156,7 +159,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State:
             context.node.get_logger().warn("Rover has no pose, waiting...")
             context.rover.send_drive_command(Twist())
             return self
-
         # If the target trajectory is empty, develop a new path to it
         if len(self.target_traj.coordinates) == 0:
             context.node.get_logger().info("Generating approach segmented path")
@@ -171,9 +173,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State:
             context=context, point=self.target_traj.get_current_point()
         ):
             context.node.get_logger().info(f"Skipped high cost point")
-            self.target_traj.increment_point()
-
-            if self.target_traj.done():
+            if self.target_traj.increment_point():
                 break
 
         if not self.target_traj.done():
@@ -236,7 +236,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State:
                 self.astar_traj.clear()
                 context.node.get_logger().info("Arrived at segment point")
                 self.target_traj.increment_point()
-
                 # If we finished the target trajectory
                 if self.target_traj.done():
                     self.target_traj.clear()
@@ -246,16 +245,13 @@ def on_loop_costmap_enabled(self, context: Context) -> State:
                         self.target_position = self.get_target_position(context)
                         return self
 
-                    # If we are within the distance threshold of the target we have finished
-                    if self.self_in_distance_threshold(context):
-                        return self.next_state(context=context, is_finished=True)
-
                     # Otherwise we need to dilate to get closer
                     else:
                         context.node.get_logger().info("Too far from target, dilating costmap")
                         if not context.shrink_dilation():
                             # Fully dilated and still failed, go to next state
-                            return self.next_state(context=context, is_finished=True)
+                            context.node.get_logger().info("Exited without distance threshold")
+                            return self.next_state(context, is_finished=True)
                         return self
 
         else:
@@ -280,7 +276,6 @@ def on_loop_costmap_disabled(self, context: Context) -> State:
             context.node.get_parameter("single_tag.stop_threshold").value,
             context.node.get_parameter("waypoint.drive_forward_threshold").value,
         )
-
         if arrived:
             if isinstance(self, LongRangeState):
                 self.target_position = self.get_target_position(context)
@@ -352,6 +347,26 @@ def on_loop(self, context: Context) -> State:
             # close so we should just return to spiral searching
             return costmap_search.CostmapSearchState()
 
+        if self.self_in_distance_threshold(context, self.looking_for_object):
+            # If we are looking for post or in no_search state or looking at an object, we can enter the done state
+            if not self.looking_for_object or self.looking_at_object(context):
+                context.node.get_logger().info("Exited through distance threshold")
+                return self.next_state(context, True)
+            # else we are not looking at the object, so we want to spin the rover
+            else:
+                context.node.get_logger().info("Within distance but not looking at object")
+                # Use the spin rover function in drive.py
+                cmd_vel, arrived = context.drive.spin_rover(
+                    context.rover.get_pose_in_map(), self.target_position, self.DISTANCE_THRESHOLD
+                )
+                # If we have finished spinning and haven't moved onto a different state, we want to enter the costmap search state
+                if arrived:
+                    return costmap_search.CostmapSearchState()
+                # else we want to have the spin drive command applied to the rover
+                else:
+                    context.rover.send_drive_command(cmd_vel)
+                    return self
+
         if self.USE_COSTMAP:
             return self.on_loop_costmap_enabled(context=context)
         else:
@@ -376,18 +391,29 @@ def display_markers(self, context: Context):
                 points=np.array([self.target_position]), color=[1.0, 1.0, 0.0], ns=str(type(self))
             )
 
-    def self_in_distance_threshold(self, context: Context):
+    def self_in_distance_threshold(self, context: Context, is_object: bool) -> bool:
         rover_SE3 = context.rover.get_pose_in_map()
         if rover_SE3 is None:
             return False
 
         target_pos = context.env.current_target_pos()
         if target_pos is None:
             return False
-
         rover_translation = rover_SE3.translation()[0:2]
+        # Calculate the distance from the rover to the target
         distance_to_target = d_calc(rover_translation, tuple(target_pos))
-        return distance_to_target < self.DISTANCE_THRESHOLD
+        # If the target isn't an object, use a smaller distance
+        if is_object:
+            return distance_to_target < self.LOOK_DISTANCE_THRESHOLD
+        else:
+            return distance_to_target < self.DISTANCE_THRESHOLD
+
+    def looking_at_object(self, context: Context) -> bool:
+        # Figure out how long its been since the TF tree published last
+        time_diff = context.env.current_time_diff()
+        if time_diff is None:
+            return False
+        return time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10**9)
 
     def point_in_distance_threshold(self, context: Context, point):
         if point is None:

navigation/context.py

@@ -106,6 +106,22 @@ def get_target_position(self, frame: str) -> np.ndarray | None:
 
         return target_pose.translation()
 
+    def get_time_diff(self, frame: str) -> None | Duration:
+        # Try to get message from TF tree with associated time
+        try:
+            _, t = SE3.from_tf_tree_with_time(self.ctx.tf_buffer, frame, self.ctx.world_frame)
+        except (
+            tf2_ros.LookupException,
+            tf2_ros.ConnectivityException,
+            tf2_ros.ExtrapolationException,
+        ):
+            return None
+
+        now = self.ctx.node.get_clock().now()
+        # Calculate difference between current time and TF tree time
+        time = Time.from_msg(t)
+        return now - time
+
     def current_target_pos(self) -> np.ndarray | None:
         assert self.ctx.course is not None
 
@@ -121,6 +137,21 @@ def current_target_pos(self) -> np.ndarray | None:
             case _:
                 return None
 
+    def current_time_diff(self):
+        assert self.ctx.course is not None
+        # Return the time difference with an associated target frame
+        match self.ctx.course.current_waypoint():
+            case Waypoint(type=WaypointType(val=WaypointType.POST), tag_id=tag_id):
+                return self.get_time_diff(f"tag{tag_id}")
+            case Waypoint(type=WaypointType(val=WaypointType.MALLET)):
+                return self.get_time_diff("hammer")
+            case Waypoint(type=WaypointType(val=WaypointType.WATER_BOTTLE)):
+                return self.get_time_diff("bottle")
+            case Waypoint(type=WaypointType(val=WaypointType.ROCK_PICK)):
+                return self.get_time_diff("pick")
+            case _:
+                return None
+
 
 class ImageTargetsStore:
     """
@@ -254,7 +285,7 @@ def look_for_post(self) -> bool:
     def look_for_object(self) -> bool:
         """
         :return: Whether the currently active waypoint is an object (if it exists).
-                 Either the mallet or the water bottle.
+                 Either the mallet, water bottle, or the rock pick.
         """
         current_waypoint = self.current_waypoint()
         return current_waypoint is not None and current_waypoint.type.val in {

navigation/drive.py

@@ -216,6 +216,13 @@ def get_drive_command(
             rover_pose,
         )
 
+    def spin_rover(self, rover_pose: SE3, target_pos: np.ndarray | None, distance_thresh: float) -> tuple[Twist, bool]:
+        if target_pos is None:
+            return Twist(), True
+        # Use the get_drive_command to spin the rover by setting the angle threshold to a tiny value
+        cmd_vel, arrived = self.get_drive_command(target_pos, rover_pose, distance_thresh, 1e-8)
+        return cmd_vel, arrived
+
     def get_default_drive_command(
         self: DriveController,
         target_pos: np.ndarray,

navigation/nav.py

@@ -89,6 +89,8 @@ def __init__(self, ctx: Context) -> None:
                 ("search.safe_approach_distance", Parameter.Type.DOUBLE),
                 ("search.angle_thresh", Parameter.Type.DOUBLE),
                 ("search.distance_threshold", Parameter.Type.DOUBLE),
+                ("search.distance_look_threshold", Parameter.Type.DOUBLE),
+                ("search.check_update_time", Parameter.Type.DOUBLE),
                 # Image Targets
                 ("image_targets.increment_weight", Parameter.Type.INTEGER),
                 ("image_targets.decrement_weight", Parameter.Type.INTEGER),