diff --git a/config/navigation.yaml b/config/navigation.yaml index 7f7f84f4..86a5dd59 100644 --- a/config/navigation.yaml +++ b/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: diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 69a24c4e..a84723b6 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -1,5 +1,4 @@ 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 @@ -7,8 +6,6 @@ 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,7 +391,7 @@ 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 @@ -384,10 +399,21 @@ def self_in_distance_threshold(self, context: Context): 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: diff --git a/navigation/context.py b/navigation/context.py index d8eac6c0..a17f0fd7 100644 --- a/navigation/context.py +++ b/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 { diff --git a/navigation/drive.py b/navigation/drive.py index ae7b20b4..f65ec07c 100644 --- a/navigation/drive.py +++ b/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, diff --git a/navigation/nav.py b/navigation/nav.py index 683b3910..ea0fe8ed 100755 --- a/navigation/nav.py +++ b/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),