From ecaa20becfafa5ed5ea4dc31f8461e391dd304d3 Mon Sep 17 00:00:00 2001 From: Vishal Date: Tue, 11 Nov 2025 20:02:37 -0500 Subject: [PATCH 01/31] changed distance function for look ahead threshold --- navigation/approach_target.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index c5fb25b7a..06a29f96d 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -19,6 +19,7 @@ class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool DISTANCE_THRESHOLD: float + LOOK_DISTANCE_THRESHOLD: float COST_INFLATION_RADIUS: float time_begin: Time astar_traj: Trajectory @@ -29,6 +30,7 @@ class ApproachTargetState(State): target_position: np.ndarray | None marker_timer: Timer update_timer: Timer + object_type: int def on_enter(self, context: Context) -> None: from .long_range import LongRangeState @@ -49,6 +51,7 @@ 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 = 5.0 self.COST_INFLATION_RADIUS = context.node.get_parameter("costmap.initial_inflation_radius").value self.marker_pub = context.node.create_publisher(Marker, "target_trajectory", 10) self.astar_traj = Trajectory(np.array([])) @@ -57,6 +60,7 @@ def on_enter(self, context: Context) -> None: self.target_position = None self.time_last_updated = context.node.get_clock().now() self.time_begin = context.node.get_clock().now() + self.object_type = current_waypoint.type.val self.marker_timer = context.node.create_timer( context.node.get_parameter("pub_path_rate").value, lambda: self.display_markers(context=context) @@ -250,7 +254,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self # If we are within the distance threshold of the target we have finished - if self.self_in_distance_threshold(context): + if self.self_in_distance_threshold(context, self.object_type): return self.next_state(context=context, is_finished=True) # Otherwise we need to dilate to get closer @@ -393,7 +397,7 @@ def display_markers(self, context: Context): ) ) - def self_in_distance_threshold(self, context: Context): + def self_in_distance_threshold(self, context: Context, object_type: int): rover_SE3 = context.rover.get_pose_in_map() if rover_SE3 is None: return False @@ -404,7 +408,10 @@ def self_in_distance_threshold(self, context: Context): rover_translation = rover_SE3.translation()[0:2] distance_to_target = d_calc(rover_translation, tuple(target_pos)) - return distance_to_target < self.DISTANCE_THRESHOLD + if(object_type == 0 or object_type == 1): + return distance_to_target < self.DISTANCE_THRESHOLD + else: + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD def point_in_distance_threshold(self, context: Context, point): if point is None: From 24da81197753238c7af248d3dda85f8a5f7bfcf2 Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 4 Dec 2025 18:26:03 -0500 Subject: [PATCH 02/31] Finished approaching target --- config/navigation.yaml | 1 + navigation/approach_target.py | 12 +++++++----- navigation/nav.py | 1 + 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index f657aadc1..78234eb78 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -67,6 +67,7 @@ navigation: safe_approach_distance: 5.0 angle_thresh: 0.0872665 #pi/36 radians / 5 degrees distance_threshold: 1.0 + distance_look_threshold: 5.0 update_delay: 3.0 single_tag: diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 06a29f96d..a3c8ce7af 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -51,7 +51,7 @@ 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 = 5.0 + self.LOOK_DISTANCE_THRESHOLD = context.node.get_parameter("search.distance_look_threshold").value self.COST_INFLATION_RADIUS = context.node.get_parameter("costmap.initial_inflation_radius").value self.marker_pub = context.node.create_publisher(Marker, "target_trajectory", 10) self.astar_traj = Trajectory(np.array([])) @@ -222,7 +222,9 @@ def on_loop_costmap_enabled(self, context: Context) -> State: else: context.node.get_logger().info("Found low-cost point") return self - + + if self.self_in_distance_threshold(context, self.object_type): + return self.next_state(context=context, is_finished=True) arrived = False cmd_vel = Twist() if not self.astar_traj.done(): @@ -233,6 +235,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: context.node.get_parameter("single_tag.stop_threshold").value, context.node.get_parameter("waypoint.drive_forward_threshold").value, ) + # If we have arrived increment the a-star trajectory if arrived: @@ -254,8 +257,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self # If we are within the distance threshold of the target we have finished - if self.self_in_distance_threshold(context, self.object_type): - return self.next_state(context=context, is_finished=True) # Otherwise we need to dilate to get closer else: @@ -287,7 +288,8 @@ 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 self.self_in_distance_threshold(context, self.object_type): + return self.next_state(context=context, is_finished=True) if arrived: if isinstance(self, LongRangeState): self.target_position = self.get_target_position(context) diff --git a/navigation/nav.py b/navigation/nav.py index bf123507e..129096340 100755 --- a/navigation/nav.py +++ b/navigation/nav.py @@ -82,6 +82,7 @@ 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), # Image Targets ("image_targets.increment_weight", Parameter.Type.INTEGER), ("image_targets.decrement_weight", Parameter.Type.INTEGER), From f64fa3a75c432ddc59df2de098dfb56f0379e024 Mon Sep 17 00:00:00 2001 From: Vishal Date: Tue, 13 Jan 2026 19:38:40 -0500 Subject: [PATCH 03/31] Deflection --- navigation/approach_target.py | 10 +- navigation/context.py | 27 ++ scripts/sim_creator.py | 251 +++++++++++++ simulator/simulator.hpp | 4 +- simulator/simulator.physics.cpp | 2 +- state_machine/state_machine.py | 2 +- urdf/arm/arm.urdf.xacro | 150 -------- urdf/meshes/arm_a.fbx | 3 - urdf/meshes/arm_b.fbx | 3 - urdf/meshes/arm_c.fbx | 3 - urdf/meshes/arm_d.fbx | 3 - urdf/meshes/arm_e.fbx | 3 - urdf/meshes/arm_gripper.fbx | 3 - urdf/meshes/bottle.fbx | 3 - urdf/meshes/ground.fbx | 3 - urdf/meshes/groundflat.fbx | 3 - urdf/meshes/hammer.fbx | 3 - urdf/meshes/primitives/cube.fbx | 3 - urdf/meshes/primitives/cylinder.fbx | 3 - urdf/meshes/primitives/sphere.fbx | 3 - urdf/meshes/rock.fbx | 3 - urdf/meshes/rover_chassis.fbx | 3 - urdf/meshes/rover_left_bogie.fbx | 3 - urdf/meshes/rover_left_rocker.fbx | 3 - urdf/meshes/rover_left_wheel.fbx | 3 - urdf/meshes/rover_right_bogie.fbx | 3 - urdf/meshes/rover_right_rocker.fbx | 3 - urdf/meshes/rover_right_wheel.fbx | 3 - urdf/meshes/tag_0.fbx | 3 - urdf/meshes/tag_1.fbx | 3 - urdf/rover/rover.urdf | 488 ------------------------- urdf/rover/rover.urdf.xacro | 359 ------------------ urdf/staging/bottle.blend | 3 - urdf/staging/ground.blend | 3 - urdf/staging/hammer.blend | 3 - urdf/staging/primitives/cube.blend | 3 - urdf/staging/primitives/cylinder.blend | 3 - urdf/staging/primitives/sphere.blend | 3 - urdf/staging/rock.blend | 3 - urdf/staging/rover.blend | 3 - urdf/staging/tag.blend | 3 - urdf/staging/zed.blend | 3 - urdf/textures/aluminum_base.png | 3 - urdf/textures/aruco_4x4_0.png | 3 - urdf/textures/aruco_4x4_1.png | 3 - urdf/textures/plastic_base.png | 3 - urdf/textures/soil_base.png | 3 - urdf/textures/wood_base.png | 3 - urdf/world/bottle.urdf.xacro | 35 -- urdf/world/hammer.urdf.xacro | 35 -- urdf/world/rock.urdf.xacro | 23 -- urdf/world/tag_0.urdf.xacro | 19 - urdf/world/tag_1.urdf.xacro | 19 - urdf/world/world.urdf.xacro | 19 - urdf/zed/include/materials.urdf.xacro | 34 -- urdf/zed/zed_descr.urdf.xacro | 38 -- urdf/zed/zed_macro.urdf.xacro | 157 -------- 57 files changed, 289 insertions(+), 1500 deletions(-) create mode 100644 scripts/sim_creator.py delete mode 100644 urdf/arm/arm.urdf.xacro delete mode 100644 urdf/meshes/arm_a.fbx delete mode 100644 urdf/meshes/arm_b.fbx delete mode 100644 urdf/meshes/arm_c.fbx delete mode 100644 urdf/meshes/arm_d.fbx delete mode 100644 urdf/meshes/arm_e.fbx delete mode 100644 urdf/meshes/arm_gripper.fbx delete mode 100644 urdf/meshes/bottle.fbx delete mode 100644 urdf/meshes/ground.fbx delete mode 100644 urdf/meshes/groundflat.fbx delete mode 100644 urdf/meshes/hammer.fbx delete mode 100644 urdf/meshes/primitives/cube.fbx delete mode 100644 urdf/meshes/primitives/cylinder.fbx delete mode 100644 urdf/meshes/primitives/sphere.fbx delete mode 100644 urdf/meshes/rock.fbx delete mode 100644 urdf/meshes/rover_chassis.fbx delete mode 100644 urdf/meshes/rover_left_bogie.fbx delete mode 100644 urdf/meshes/rover_left_rocker.fbx delete mode 100644 urdf/meshes/rover_left_wheel.fbx delete mode 100644 urdf/meshes/rover_right_bogie.fbx delete mode 100644 urdf/meshes/rover_right_rocker.fbx delete mode 100644 urdf/meshes/rover_right_wheel.fbx delete mode 100644 urdf/meshes/tag_0.fbx delete mode 100644 urdf/meshes/tag_1.fbx delete mode 100644 urdf/rover/rover.urdf delete mode 100644 urdf/rover/rover.urdf.xacro delete mode 100644 urdf/staging/bottle.blend delete mode 100644 urdf/staging/ground.blend delete mode 100644 urdf/staging/hammer.blend delete mode 100644 urdf/staging/primitives/cube.blend delete mode 100644 urdf/staging/primitives/cylinder.blend delete mode 100644 urdf/staging/primitives/sphere.blend delete mode 100644 urdf/staging/rock.blend delete mode 100644 urdf/staging/rover.blend delete mode 100644 urdf/staging/tag.blend delete mode 100644 urdf/staging/zed.blend delete mode 100644 urdf/textures/aluminum_base.png delete mode 100644 urdf/textures/aruco_4x4_0.png delete mode 100644 urdf/textures/aruco_4x4_1.png delete mode 100644 urdf/textures/plastic_base.png delete mode 100644 urdf/textures/soil_base.png delete mode 100644 urdf/textures/wood_base.png delete mode 100644 urdf/world/bottle.urdf.xacro delete mode 100644 urdf/world/hammer.urdf.xacro delete mode 100644 urdf/world/rock.urdf.xacro delete mode 100644 urdf/world/tag_0.urdf.xacro delete mode 100644 urdf/world/tag_1.urdf.xacro delete mode 100644 urdf/world/world.urdf.xacro delete mode 100644 urdf/zed/include/materials.urdf.xacro delete mode 100644 urdf/zed/zed_descr.urdf.xacro delete mode 100644 urdf/zed/zed_macro.urdf.xacro diff --git a/navigation/approach_target.py b/navigation/approach_target.py index a3c8ce7af..6d7b40863 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -31,6 +31,7 @@ class ApproachTargetState(State): marker_timer: Timer update_timer: Timer object_type: int + no_look_ahead_dict: dict def on_enter(self, context: Context) -> None: from .long_range import LongRangeState @@ -61,6 +62,7 @@ def on_enter(self, context: Context) -> None: self.time_last_updated = context.node.get_clock().now() self.time_begin = context.node.get_clock().now() self.object_type = current_waypoint.type.val + self.no_look_ahead_dict = {'NO_SEARCH': 0, 'POST': 1} self.marker_timer = context.node.create_timer( context.node.get_parameter("pub_path_rate").value, lambda: self.display_markers(context=context) @@ -407,13 +409,15 @@ def self_in_distance_threshold(self, context: Context, object_type: int): target_pos = context.env.current_target_pos() if target_pos is None: return False - + time_diff = context.env.current_time_diff() + if time_diff is None: + return False rover_translation = rover_SE3.translation()[0:2] distance_to_target = d_calc(rover_translation, tuple(target_pos)) - if(object_type == 0 or object_type == 1): + if(object_type in self.no_look_ahead_dict.values()): return distance_to_target < self.DISTANCE_THRESHOLD else: - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD and time_diff > Duration(nanoseconds=50000000) def point_in_distance_threshold(self, context: Context, point): if point is None: diff --git a/navigation/context.py b/navigation/context.py index 0d419f6f4..ac9b8dad3 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -102,6 +102,20 @@ def get_target_position(self, frame: str) -> np.ndarray | None: return None return target_pose.translation() + + def get_time_diff(self, frame: str) -> None | Time: + try: + waste, 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() + time = Time.from_msg(t) + return now - time def current_target_pos(self) -> np.ndarray | None: assert self.ctx.course is not None @@ -115,6 +129,19 @@ def current_target_pos(self) -> np.ndarray | None: return self.get_target_position("bottle") case _: return None + + def current_time_diff(self): + assert self.ctx.course is not None + + 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 _: + return None class ImageTargetsStore: diff --git a/scripts/sim_creator.py b/scripts/sim_creator.py new file mode 100644 index 000000000..8dbcdc9a5 --- /dev/null +++ b/scripts/sim_creator.py @@ -0,0 +1,251 @@ +import tkinter as tk + + +def create_60x60_grid(): + root = tk.Tk() + root.title("30×30 Grid with Toggleable Colors (including black)") + + # ------------------- + # CONFIGURATION + # ------------------- + GRID_SIZE = 35 # 30 cells in each dimension + CELL_SIZE = 25 # Each cell is 30×30 pixels + MARGIN = 30 # Extra space around the grid for the border + TOTAL_PIXELS = GRID_SIZE * CELL_SIZE + + # Now includes "black" to represent the rover's position + color_modes = ["white", "green", "blue", "red"] + current_mode_index = 0 # Start with mode = "white" + + # 2D array to keep track of each cell's color (initially "white") + grid_colors = [["white" for _ in range(GRID_SIZE)] for __ in range(GRID_SIZE)] + + # Include black → 4 + color_key = {"white": 0, "green": 1, "blue": 2, "red": 3} + + # ------------------- + # TK WIDGETS + # ------------------- + main_frame = tk.Frame(root) + main_frame.pack() + + # Increase the Canvas size to accommodate margins + CANVAS_SIZE = TOTAL_PIXELS + 2 * MARGIN + + # Left: Canvas for the 30×30 grid + canvas = tk.Canvas(main_frame, width=CANVAS_SIZE, height=CANVAS_SIZE, bg="white") + canvas.pack(side=tk.LEFT) + + # Right: A small canvas (icon) to show current mode color + icon_size = 50 + mode_canvas = tk.Canvas(main_frame, width=icon_size, height=icon_size, bg="white") + mode_canvas.pack(side=tk.RIGHT, padx=10) + + # Draw a rectangle showing the current mode color + mode_rect = mode_canvas.create_rectangle( + 0, 0, icon_size, icon_size, fill=color_modes[current_mode_index], outline="black" + ) + + def update_mode_indicator(): + """Update the color of the 'mode' icon.""" + mode_canvas.itemconfig(mode_rect, fill=color_modes[current_mode_index]) + + # ------------------- + # CREATE THE GRID + # ------------------- + rect_ids = {} # dict: (row, col) -> rectangle_id + + for row in range(GRID_SIZE): + for col in range(GRID_SIZE): + x1 = MARGIN + col * CELL_SIZE + y1 = MARGIN + row * CELL_SIZE + x2 = x1 + CELL_SIZE + y2 = y1 + CELL_SIZE + + rect_id = canvas.create_rectangle(x1, y1, x2, y2, fill="white", outline="black") + rect_ids[(row, col)] = rect_id + + # ------------------- + # EVENT HANDLERS + # ------------------- + def on_click(event): + """ + When the user clicks on a cell, toggle it between white + and the current mode color. + """ + x, y = event.x, event.y + # Check if the click is inside the grid + if not (MARGIN <= x < MARGIN + GRID_SIZE * CELL_SIZE and MARGIN <= y < MARGIN + GRID_SIZE * CELL_SIZE): + return + + col_clicked = (x - MARGIN) // CELL_SIZE + row_clicked = (y - MARGIN) // CELL_SIZE + + rect_id = rect_ids[(row_clicked, col_clicked)] + current_fill = grid_colors[row_clicked][col_clicked] + desired_fill = color_modes[current_mode_index] + + # Toggle logic: if it's already the current mode color, revert to white + # otherwise set it to current mode color + if current_fill == desired_fill: + new_fill = "white" + else: + new_fill = desired_fill + + # Update + canvas.itemconfig(rect_id, fill=new_fill) + grid_colors[row_clicked][col_clicked] = new_fill + + canvas.bind("", on_click) + + def cycle_mode(event): + """ + Pressing SPACE cycles to the next color mode. + """ + nonlocal current_mode_index + current_mode_index = (current_mode_index + 1) % len(color_modes) + update_mode_indicator() + + # ------------------- + # HOVER COORDINATES + # ------------------- + hover_label = tk.Label(root, text="Hovering at: (N/A, N/A)") + hover_label.pack() + + def on_hover(event): + """ + Update the label with the current grid cell coordinates where the mouse is hovering, + using (0, 0) as the center of the grid and aligning coordinates with Cartesian system. + """ + x, y = event.x, event.y + if MARGIN <= x < MARGIN + GRID_SIZE * CELL_SIZE and MARGIN <= y < MARGIN + GRID_SIZE * CELL_SIZE: + col_hovered = (x - MARGIN) // CELL_SIZE + row_hovered = (y - MARGIN) // CELL_SIZE + + # Convert to Cartesian-like coordinates + center_offset = GRID_SIZE // 2 + adjusted_x = col_hovered - center_offset # X-coordinate + adjusted_y = center_offset - row_hovered # Y-coordinate + + hover_label.config(text=f"Hovering at: ({adjusted_x}, {adjusted_y})") + else: + hover_label.config(text="Hovering at: (N/A, N/A)") + + canvas.bind("", on_hover) + + def on_q_press(event): + """ + Pressing 'q' prints the 2D array (using color_key), then creates new_sim.yaml, + and closes the window. + """ + # 1) Print the 2D array + numeric_grid = [] + for r in range(GRID_SIZE): + numeric_row = [color_key[color] for color in grid_colors[r]] + numeric_grid.append(numeric_row) + print(numeric_row) + + # 2) Prepare YAML header + yaml_header = """# All units are in SI +# =================== +# Time: second, hz +# Angle: radian +# Distance: meter + +simulator: + ros__parameters: + save_rate: 1.0 + save_history: 4096 + headless: false + + ref_heading: 90.0 # For the GPS sensor to work + + objects: + rover: + type: urdf + uri: package://mrover/urdf/rover/rover.urdf.xacro + position: [ 0.0, 0.0, 0.1 ] + world: + type: urdf + uri: package://mrover/urdf/world/world.urdf.xacro + bottle: + type: urdf + uri: package://mrover/urdf/world/bottle.urdf.xacro + position: [9.0, 10.0, 0.5] + +""" + + # 3) Generate objects for rocks, ignoring cells with value 0 (white or no rock). + rock_counter = 1 + yaml_rocks = [] + + # URIs by value: 1 => small, 2 => medium, 3 => large + uri_map = { + 1: "package://mrover/urdf/world/small_rock.urdf.xacro", + 2: "package://mrover/urdf/world/medium_rock.urdf.xacro", + 3: "package://mrover/urdf/world/large_rock.urdf.xacro", + } + # For demonstration, different z's by size + z_map = {1: "0.5", 2: "1.0", 3: "1.0"} + + center_offset = GRID_SIZE // 2 # Center offset for coordinate transformation + + for row_i in range(GRID_SIZE): + for col_i in range(GRID_SIZE): + val = numeric_grid[row_i][col_i] + # Only create a rock if val in {1,2,3} + if val in uri_map: + # Adjust coordinates for the YAML output + x = col_i - center_offset # X-coordinate + y = center_offset - row_i # Y-coordinate (invert Y-axis) + rock_name = f"rock_{rock_counter}" + rock_counter += 1 + + lines = [ + f" {rock_name}:", + f" type: urdf", + f" uri: {uri_map[val]}", + f" position: [ {x:.2f}, {y:.2f}, {z_map[val]} ]\n", + ] + yaml_rocks.append("\n".join(lines)) + + # 4) Add finishing lines + yaml_footer = """ ref_lat: 38.4225202 + ref_lon: -110.7844653 + ref_alt: 0.0 + world_frame: "map" + rover_frame: "sim_base_link" +""" + + # 5) Write out to new_sim.yaml + with open("config/simulator.yaml", "w") as f: + f.write(yaml_header) + if yaml_rocks: + f.write(" # Auto-generated rocks from the grid\n") + f.write("\n".join(yaml_rocks)) + f.write("\n") + f.write(yaml_footer) + + print("YAML file successfully written to config/simulator.yaml.") + # Close the window + root.destroy() + + # Bind events + root.bind("", cycle_mode) + root.bind("q", on_q_press) + + # ------------------- + # LABEL IN CENTER + # ------------------- + center = GRID_SIZE // 2 + center_x = MARGIN + center * CELL_SIZE + (CELL_SIZE // 2) + center_y = MARGIN + center * CELL_SIZE + (CELL_SIZE // 2) + canvas.create_text(center_x, center_y, text="(0,0)", fill="black", anchor="center") + + # Show the initial mode color in the icon + update_mode_indicator() + root.mainloop() + + +if __name__ == "__main__": + create_60x60_grid() diff --git a/simulator/simulator.hpp b/simulator/simulator.hpp index 24c6fe7b6..164efb952 100644 --- a/simulator/simulator.hpp +++ b/simulator/simulator.hpp @@ -233,8 +233,8 @@ namespace mrover { bool mEnablePhysics{}; bool mRenderModels = true; bool mRenderWireframeColliders = false; - double mPublishHammerDistanceThreshold = 3; - double mPublishBottleDistanceThreshold = 3; + double mPublishHammerDistanceThreshold = 6.0; + double mPublishBottleDistanceThreshold = 6.0; float mCameraLockSlerp = 0.02; float mFloat = 0.0f; diff --git a/simulator/simulator.physics.cpp b/simulator/simulator.physics.cpp index 1696af7d7..52363fc9e 100644 --- a/simulator/simulator.physics.cpp +++ b/simulator/simulator.physics.cpp @@ -13,7 +13,7 @@ namespace mrover { // Important formula that needs to hold true to avoid dropping: timeStep < maxSubSteps * fixedTimeStep constexpr int MAX_SUB_STEPS = 1024; - constexpr double TAU = 2 * std::numbers::pi; + constexpr double TAU = 0.5 * std::numbers::pi; auto btTransformToSe3(btTransform const& transform) -> SE3d { btVector3 const& p = transform.getOrigin(); diff --git a/state_machine/state_machine.py b/state_machine/state_machine.py index 486528db9..58e4e090d 100644 --- a/state_machine/state_machine.py +++ b/state_machine/state_machine.py @@ -66,7 +66,7 @@ def update(self): self.current_state = next_state self.current_state.on_enter(self.context) except Exception as e: - self.logger.debug(f"Error in {str(current_state)}: {e}") + self.logger.warn(f"Error in {str(current_state)}: {e}") def add_transition(self, state_from: State, state_to: State) -> None: self.state_transitions[type(state_from)].add(type(state_to)) diff --git a/urdf/arm/arm.urdf.xacro b/urdf/arm/arm.urdf.xacro deleted file mode 100644 index 62106e436..000000000 --- a/urdf/arm/arm.urdf.xacro +++ /dev/null @@ -1,150 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/meshes/arm_a.fbx b/urdf/meshes/arm_a.fbx deleted file mode 100644 index 8f7c71b41..000000000 --- a/urdf/meshes/arm_a.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d72481ca801642e73fea847317225514516d10667d777c0287021a90c77befba -size 869948 diff --git a/urdf/meshes/arm_b.fbx b/urdf/meshes/arm_b.fbx deleted file mode 100644 index 59968f67e..000000000 --- a/urdf/meshes/arm_b.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:27acf7d3fe472656b65d6185876002487b03a3aa6f26aea2bc2c49525dd5df3e -size 1128780 diff --git a/urdf/meshes/arm_c.fbx b/urdf/meshes/arm_c.fbx deleted file mode 100644 index 37c8aa17a..000000000 --- a/urdf/meshes/arm_c.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:14f152298e48eb526c1d28c96d3342a8e9ae0e449a5824026bb9bc36c07a5a6e -size 885340 diff --git a/urdf/meshes/arm_d.fbx b/urdf/meshes/arm_d.fbx deleted file mode 100644 index be192a925..000000000 --- a/urdf/meshes/arm_d.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d270eeab807b552603478d239cbe1d5dc0cc374c11cfd8aff8d49b78340df9d3 -size 316844 diff --git a/urdf/meshes/arm_e.fbx b/urdf/meshes/arm_e.fbx deleted file mode 100644 index 0eb040dcc..000000000 --- a/urdf/meshes/arm_e.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8c9c68ef7fd225bc93d933c6429a4b1994cfe099c27a13eb4d2c502146e49331 -size 407180 diff --git a/urdf/meshes/arm_gripper.fbx b/urdf/meshes/arm_gripper.fbx deleted file mode 100644 index 2ed35c5a1..000000000 --- a/urdf/meshes/arm_gripper.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:32a03cd4a738329e8311b0f27199171ed6a850c333d618b33527c42f8a9aa990 -size 190156 diff --git a/urdf/meshes/bottle.fbx b/urdf/meshes/bottle.fbx deleted file mode 100644 index 5602ef9c2..000000000 --- a/urdf/meshes/bottle.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a6eda3d1f92892e01fcf2ad3dff4421d59ef1f4688fbe8e7e700151065a5b6be -size 17596 diff --git a/urdf/meshes/ground.fbx b/urdf/meshes/ground.fbx deleted file mode 100644 index 1e06dd797..000000000 --- a/urdf/meshes/ground.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:eb9df0b23202bdbaf6319b67e2e723197edec84fba09b53ee2c18ffbdf454626 -size 39708 diff --git a/urdf/meshes/groundflat.fbx b/urdf/meshes/groundflat.fbx deleted file mode 100644 index aca631c50..000000000 --- a/urdf/meshes/groundflat.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5d1c9d3030e2c1a5688101b93b95c35cac89e10f2951d6844a9834f9ffc4f92b -size 18972 diff --git a/urdf/meshes/hammer.fbx b/urdf/meshes/hammer.fbx deleted file mode 100644 index 9f8b6b9e4..000000000 --- a/urdf/meshes/hammer.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4b5e36777f78ac597bcd49076a0cf2b674d886c315b50b5d1ff75c7bcea3123a -size 16620 diff --git a/urdf/meshes/primitives/cube.fbx b/urdf/meshes/primitives/cube.fbx deleted file mode 100644 index 9326aa8ca..000000000 --- a/urdf/meshes/primitives/cube.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3fc4c443d8ae8ca4cca8d98e6aa37a811db6fc23ce4eb169abc3314625a5f27a -size 11756 diff --git a/urdf/meshes/primitives/cylinder.fbx b/urdf/meshes/primitives/cylinder.fbx deleted file mode 100644 index 02941d306..000000000 --- a/urdf/meshes/primitives/cylinder.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4678acc34ec93779fa178d326f321b8cc67976c7e8df754c88fea29b34ee0239 -size 12460 diff --git a/urdf/meshes/primitives/sphere.fbx b/urdf/meshes/primitives/sphere.fbx deleted file mode 100644 index 07b9cab65..000000000 --- a/urdf/meshes/primitives/sphere.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b5eb2df5136ad610d75ba102a17a9bee8fbb0cb253a45cc6e8bad1c527559c25 -size 13948 diff --git a/urdf/meshes/rock.fbx b/urdf/meshes/rock.fbx deleted file mode 100644 index 0bd101bbe..000000000 --- a/urdf/meshes/rock.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8098184699873dfc775269d78e3a2d069344d89b69fe67d38d06a3dba6f92c4d -size 18940 diff --git a/urdf/meshes/rover_chassis.fbx b/urdf/meshes/rover_chassis.fbx deleted file mode 100644 index 039d2bb40..000000000 --- a/urdf/meshes/rover_chassis.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fc1a2241d36784b9328ca074009915df5b9caf1926daf85086a4d72a9db8529c -size 1967532 diff --git a/urdf/meshes/rover_left_bogie.fbx b/urdf/meshes/rover_left_bogie.fbx deleted file mode 100644 index a50ee8540..000000000 --- a/urdf/meshes/rover_left_bogie.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3c5ecacbf35e5a24e0f6530972d579c2171fd67f87fa727960630e2cd697e47e -size 1574524 diff --git a/urdf/meshes/rover_left_rocker.fbx b/urdf/meshes/rover_left_rocker.fbx deleted file mode 100644 index c0af1b801..000000000 --- a/urdf/meshes/rover_left_rocker.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7da666d555f7b32538a016f6cceb1bb45f625f0b6ae15c0a43e0eea4c753c4e0 -size 1064220 diff --git a/urdf/meshes/rover_left_wheel.fbx b/urdf/meshes/rover_left_wheel.fbx deleted file mode 100644 index ab6089917..000000000 --- a/urdf/meshes/rover_left_wheel.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:13129b734d971654de759255f9a6e65c2571bab60ad668a2c325f1a92b377612 -size 556396 diff --git a/urdf/meshes/rover_right_bogie.fbx b/urdf/meshes/rover_right_bogie.fbx deleted file mode 100644 index 3c161339f..000000000 --- a/urdf/meshes/rover_right_bogie.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2329d7c9272175888328413b929e51a7896a103a701adea225281b1ecaebb80c -size 1572668 diff --git a/urdf/meshes/rover_right_rocker.fbx b/urdf/meshes/rover_right_rocker.fbx deleted file mode 100644 index 28cc1dc5d..000000000 --- a/urdf/meshes/rover_right_rocker.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ce1307454a58a91b5bbf431cfa4c1bf289b62164595ed5c7d54b4af259f701b0 -size 1064460 diff --git a/urdf/meshes/rover_right_wheel.fbx b/urdf/meshes/rover_right_wheel.fbx deleted file mode 100644 index bb5022926..000000000 --- a/urdf/meshes/rover_right_wheel.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:201aaec237d10542c6a97ee8afd7b7bca7c01b6463952872e22a3369b31fa3cb -size 550204 diff --git a/urdf/meshes/tag_0.fbx b/urdf/meshes/tag_0.fbx deleted file mode 100644 index 328f38944..000000000 --- a/urdf/meshes/tag_0.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ee8c37f9ab9725905679ef6a042378a22266302f18bfabfd4c1cf0914961e375 -size 18236 diff --git a/urdf/meshes/tag_1.fbx b/urdf/meshes/tag_1.fbx deleted file mode 100644 index 2a877464f..000000000 --- a/urdf/meshes/tag_1.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:330cdfb6bd7338770028913dcc2ae1a48afe47b4a663de58b62d2e28b448b83d -size 18236 diff --git a/urdf/rover/rover.urdf b/urdf/rover/rover.urdf deleted file mode 100644 index 8d2d6eef5..000000000 --- a/urdf/rover/rover.urdf +++ /dev/null @@ -1,488 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro deleted file mode 100644 index b5fdb4a1c..000000000 --- a/urdf/rover/rover.urdf.xacro +++ /dev/null @@ -1,359 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/staging/bottle.blend b/urdf/staging/bottle.blend deleted file mode 100644 index db12f4aff..000000000 --- a/urdf/staging/bottle.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d4755b6ead5cec56f3b044ba4e48280fbddd3eefc481030ae56af4e32668c91b -size 933840 diff --git a/urdf/staging/ground.blend b/urdf/staging/ground.blend deleted file mode 100644 index 6f771f148..000000000 --- a/urdf/staging/ground.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:61158266eef1f35d53ae11a19fb2dff065f3b55cd379e136bd576615feb4721a -size 998428 diff --git a/urdf/staging/hammer.blend b/urdf/staging/hammer.blend deleted file mode 100644 index 9acb014c4..000000000 --- a/urdf/staging/hammer.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ce6398576246663ac1aeca8d03fe88fd9db0843b035bc6a63f85019b79645dee -size 908620 diff --git a/urdf/staging/primitives/cube.blend b/urdf/staging/primitives/cube.blend deleted file mode 100644 index 1433c642b..000000000 --- a/urdf/staging/primitives/cube.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a09c7e9cb4693adba6f584b9b59529a92b5c9f187ced25a889b6f8d4bee31993 -size 882484 diff --git a/urdf/staging/primitives/cylinder.blend b/urdf/staging/primitives/cylinder.blend deleted file mode 100644 index b9494594e..000000000 --- a/urdf/staging/primitives/cylinder.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bb61d2ba5b0e89acbde4455e69751a51133276db0d7bef3651119368b90d29ca -size 886544 diff --git a/urdf/staging/primitives/sphere.blend b/urdf/staging/primitives/sphere.blend deleted file mode 100644 index ecd3fa669..000000000 --- a/urdf/staging/primitives/sphere.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:26f4ea41ada2d7dbd3a880551ef82c0d240add487fe610ca7459195a1121d506 -size 889868 diff --git a/urdf/staging/rock.blend b/urdf/staging/rock.blend deleted file mode 100644 index 225b42619..000000000 --- a/urdf/staging/rock.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:58f34c806bf5938d1da0956943cacbf68998e596d90579e72f06cd845c0ceb29 -size 893320 diff --git a/urdf/staging/rover.blend b/urdf/staging/rover.blend deleted file mode 100644 index bd604d226..000000000 --- a/urdf/staging/rover.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:eb496230d3a997f741fabaf401e493432ad6ea09292c5d011edd14b1118b0490 -size 24983740 diff --git a/urdf/staging/tag.blend b/urdf/staging/tag.blend deleted file mode 100644 index 04292f1cd..000000000 --- a/urdf/staging/tag.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:152d65ea87066650aa5882b1985b993f1bb2f0ba417eb4a7c129c20a2458264d -size 898808 diff --git a/urdf/staging/zed.blend b/urdf/staging/zed.blend deleted file mode 100644 index 93eac6282..000000000 --- a/urdf/staging/zed.blend +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:808955b4fc7802d2c7c40f400fbff598cff4777465aeed4255e2c7959f748fd5 -size 931268 diff --git a/urdf/textures/aluminum_base.png b/urdf/textures/aluminum_base.png deleted file mode 100644 index 9163aa9b8..000000000 --- a/urdf/textures/aluminum_base.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:13b7f63c4bf9328869dcfe1e346ddf5375de51dcf078ba385fdb222267a890d4 -size 582262 diff --git a/urdf/textures/aruco_4x4_0.png b/urdf/textures/aruco_4x4_0.png deleted file mode 100644 index 0495d2249..000000000 --- a/urdf/textures/aruco_4x4_0.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5810218d0ce8036dabca15d6df95b73fb89da32eeaedbc1cd9e422ece80bef9d -size 29914 diff --git a/urdf/textures/aruco_4x4_1.png b/urdf/textures/aruco_4x4_1.png deleted file mode 100644 index 3f6409389..000000000 --- a/urdf/textures/aruco_4x4_1.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6c8b2e6554ef695f04d2d6ec099e42220f5aa8837b5357389671bd1af3702708 -size 25967 diff --git a/urdf/textures/plastic_base.png b/urdf/textures/plastic_base.png deleted file mode 100644 index 8608dcd06..000000000 --- a/urdf/textures/plastic_base.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fd9685aa0d5ecd23168906150a16fd6e007f74c92f3f4becc4c7a1879d4a93ad -size 71611 diff --git a/urdf/textures/soil_base.png b/urdf/textures/soil_base.png deleted file mode 100644 index 7f0e1d484..000000000 --- a/urdf/textures/soil_base.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8f55e0ab712d74af9a48551496f4c51a71264fd3d69e8b01e60a0651277e5e34 -size 5169047 diff --git a/urdf/textures/wood_base.png b/urdf/textures/wood_base.png deleted file mode 100644 index 27f74c3b7..000000000 --- a/urdf/textures/wood_base.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2d079a0bea287e2b167eaec78360932aa822e970b4b9650c7c4f55bbdf36a91f -size 5272102 diff --git a/urdf/world/bottle.urdf.xacro b/urdf/world/bottle.urdf.xacro deleted file mode 100644 index 88b716536..000000000 --- a/urdf/world/bottle.urdf.xacro +++ /dev/null @@ -1,35 +0,0 @@ - - - \ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/hammer.urdf.xacro b/urdf/world/hammer.urdf.xacro deleted file mode 100644 index d591136e1..000000000 --- a/urdf/world/hammer.urdf.xacro +++ /dev/null @@ -1,35 +0,0 @@ - - - \ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/rock.urdf.xacro b/urdf/world/rock.urdf.xacro deleted file mode 100644 index c4de4f332..000000000 --- a/urdf/world/rock.urdf.xacro +++ /dev/null @@ -1,23 +0,0 @@ - - - \ - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/tag_0.urdf.xacro b/urdf/world/tag_0.urdf.xacro deleted file mode 100644 index b081eb58d..000000000 --- a/urdf/world/tag_0.urdf.xacro +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/tag_1.urdf.xacro b/urdf/world/tag_1.urdf.xacro deleted file mode 100644 index 6c0afe34f..000000000 --- a/urdf/world/tag_1.urdf.xacro +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/world.urdf.xacro b/urdf/world/world.urdf.xacro deleted file mode 100644 index 7bd248c56..000000000 --- a/urdf/world/world.urdf.xacro +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/urdf/zed/include/materials.urdf.xacro b/urdf/zed/include/materials.urdf.xacro deleted file mode 100644 index 8ccb50b18..000000000 --- a/urdf/zed/include/materials.urdf.xacro +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/urdf/zed/zed_descr.urdf.xacro b/urdf/zed/zed_descr.urdf.xacro deleted file mode 100644 index cff4fd376..000000000 --- a/urdf/zed/zed_descr.urdf.xacro +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/zed/zed_macro.urdf.xacro b/urdf/zed/zed_macro.urdf.xacro deleted file mode 100644 index 3c852d45c..000000000 --- a/urdf/zed/zed_macro.urdf.xacro +++ /dev/null @@ -1,157 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 2ac265566f1607577c0b5550ecdacfb81fae3cd4 Mon Sep 17 00:00:00 2001 From: Vishal Date: Sun, 18 Jan 2026 13:35:45 -0500 Subject: [PATCH 04/31] Fixed distance to target error --- navigation/approach_target.py | 4 +- urdf/arm/arm.urdf.xacro | 150 +++++++ urdf/meshes/arm_a.fbx | 3 + urdf/meshes/arm_b.fbx | 3 + urdf/meshes/arm_c.fbx | 3 + urdf/meshes/arm_d.fbx | 3 + urdf/meshes/arm_e.fbx | 3 + urdf/meshes/arm_gripper.fbx | 3 + urdf/meshes/bottle.fbx | 3 + urdf/meshes/ground.fbx | 3 + urdf/meshes/groundflat.fbx | 3 + urdf/meshes/hammer.fbx | 3 + urdf/meshes/large_rock.fbx | 3 + urdf/meshes/large_rock_decimated.fbx | 3 + urdf/meshes/medium_rock.fbx | 3 + urdf/meshes/medium_rock_decimated.fbx | 3 + urdf/meshes/primitives/cube.fbx | 3 + urdf/meshes/primitives/cylinder.fbx | 3 + urdf/meshes/primitives/sphere.fbx | 3 + urdf/meshes/rock.fbx | 3 + urdf/meshes/rover_chassis.fbx | 3 + urdf/meshes/rover_left_bogie.fbx | 3 + urdf/meshes/rover_left_rocker.fbx | 3 + urdf/meshes/rover_left_wheel.fbx | 3 + urdf/meshes/rover_right_bogie.fbx | 3 + urdf/meshes/rover_right_rocker.fbx | 3 + urdf/meshes/rover_right_wheel.fbx | 3 + urdf/meshes/small_rock.fbx | 3 + urdf/meshes/small_rock_decimated.fbx | 3 + urdf/meshes/tag_0.fbx | 3 + urdf/meshes/tag_1.fbx | 3 + urdf/meshes/textured_ground.fbx | 3 + urdf/meshes/textured_ground_decimated.fbx | 3 + urdf/rover/rover.urdf | 488 ++++++++++++++++++++++ urdf/staging/bottle.blend | 3 + urdf/staging/ground.blend | 3 + urdf/staging/hammer.blend | 3 + urdf/staging/primitives/cube.blend | 3 + urdf/staging/primitives/cylinder.blend | 3 + urdf/staging/primitives/sphere.blend | 3 + urdf/staging/rock.blend | 3 + urdf/staging/rover.blend | 3 + urdf/staging/tag.blend | 3 + urdf/staging/zed.blend | 3 + urdf/textures/aluminum_base.png | 3 + urdf/textures/aruco_4x4_0.png | 3 + urdf/textures/aruco_4x4_1.png | 3 + urdf/textures/clay-rock.jpg | Bin 0 -> 22869 bytes urdf/textures/plastic_base.png | 3 + urdf/textures/soil_base.png | 3 + urdf/textures/wood_base.png | 3 + urdf/world/bottle.urdf.xacro | 35 ++ urdf/world/bumpy_world.urdf.xacro | 19 + urdf/world/hammer.urdf.xacro | 35 ++ urdf/world/large_rock.urdf.xacro | 23 + urdf/world/medium_rock.urdf.xacro | 23 + urdf/world/rock.urdf.xacro | 23 + urdf/world/small_rock.urdf.xacro | 23 + urdf/world/tag_0.urdf.xacro | 19 + urdf/world/tag_1.urdf.xacro | 19 + urdf/world/world.urdf.xacro | 19 + urdf/zed/include/materials.urdf.xacro | 34 ++ urdf/zed/zed_descr.urdf.xacro | 38 ++ urdf/zed/zed_macro.urdf.xacro | 157 +++++++ 64 files changed, 1248 insertions(+), 2 deletions(-) create mode 100644 urdf/arm/arm.urdf.xacro create mode 100644 urdf/meshes/arm_a.fbx create mode 100644 urdf/meshes/arm_b.fbx create mode 100644 urdf/meshes/arm_c.fbx create mode 100644 urdf/meshes/arm_d.fbx create mode 100644 urdf/meshes/arm_e.fbx create mode 100644 urdf/meshes/arm_gripper.fbx create mode 100644 urdf/meshes/bottle.fbx create mode 100644 urdf/meshes/ground.fbx create mode 100644 urdf/meshes/groundflat.fbx create mode 100644 urdf/meshes/hammer.fbx create mode 100644 urdf/meshes/large_rock.fbx create mode 100644 urdf/meshes/large_rock_decimated.fbx create mode 100644 urdf/meshes/medium_rock.fbx create mode 100644 urdf/meshes/medium_rock_decimated.fbx create mode 100644 urdf/meshes/primitives/cube.fbx create mode 100644 urdf/meshes/primitives/cylinder.fbx create mode 100644 urdf/meshes/primitives/sphere.fbx create mode 100644 urdf/meshes/rock.fbx create mode 100644 urdf/meshes/rover_chassis.fbx create mode 100644 urdf/meshes/rover_left_bogie.fbx create mode 100644 urdf/meshes/rover_left_rocker.fbx create mode 100644 urdf/meshes/rover_left_wheel.fbx create mode 100644 urdf/meshes/rover_right_bogie.fbx create mode 100644 urdf/meshes/rover_right_rocker.fbx create mode 100644 urdf/meshes/rover_right_wheel.fbx create mode 100644 urdf/meshes/small_rock.fbx create mode 100644 urdf/meshes/small_rock_decimated.fbx create mode 100644 urdf/meshes/tag_0.fbx create mode 100644 urdf/meshes/tag_1.fbx create mode 100644 urdf/meshes/textured_ground.fbx create mode 100644 urdf/meshes/textured_ground_decimated.fbx create mode 100644 urdf/rover/rover.urdf create mode 100644 urdf/staging/bottle.blend create mode 100644 urdf/staging/ground.blend create mode 100644 urdf/staging/hammer.blend create mode 100644 urdf/staging/primitives/cube.blend create mode 100644 urdf/staging/primitives/cylinder.blend create mode 100644 urdf/staging/primitives/sphere.blend create mode 100644 urdf/staging/rock.blend create mode 100644 urdf/staging/rover.blend create mode 100644 urdf/staging/tag.blend create mode 100644 urdf/staging/zed.blend create mode 100644 urdf/textures/aluminum_base.png create mode 100644 urdf/textures/aruco_4x4_0.png create mode 100644 urdf/textures/aruco_4x4_1.png create mode 100644 urdf/textures/clay-rock.jpg create mode 100644 urdf/textures/plastic_base.png create mode 100644 urdf/textures/soil_base.png create mode 100644 urdf/textures/wood_base.png create mode 100644 urdf/world/bottle.urdf.xacro create mode 100644 urdf/world/bumpy_world.urdf.xacro create mode 100644 urdf/world/hammer.urdf.xacro create mode 100644 urdf/world/large_rock.urdf.xacro create mode 100644 urdf/world/medium_rock.urdf.xacro create mode 100644 urdf/world/rock.urdf.xacro create mode 100644 urdf/world/small_rock.urdf.xacro create mode 100644 urdf/world/tag_0.urdf.xacro create mode 100644 urdf/world/tag_1.urdf.xacro create mode 100644 urdf/world/world.urdf.xacro create mode 100644 urdf/zed/include/materials.urdf.xacro create mode 100644 urdf/zed/zed_descr.urdf.xacro create mode 100644 urdf/zed/zed_macro.urdf.xacro diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 6d7b40863..96ea70f0b 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -413,11 +413,11 @@ def self_in_distance_threshold(self, context: Context, object_type: int): if time_diff is None: return False rover_translation = rover_SE3.translation()[0:2] - distance_to_target = d_calc(rover_translation, tuple(target_pos)) + distance_to_target = d_calc(rover_translation, tuple(target_pos)) if(object_type in self.no_look_ahead_dict.values()): return distance_to_target < self.DISTANCE_THRESHOLD else: - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD and time_diff > Duration(nanoseconds=50000000) + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD and time_diff < Duration(nanoseconds=50000000) def point_in_distance_threshold(self, context: Context, point): if point is None: diff --git a/urdf/arm/arm.urdf.xacro b/urdf/arm/arm.urdf.xacro new file mode 100644 index 000000000..62106e436 --- /dev/null +++ b/urdf/arm/arm.urdf.xacro @@ -0,0 +1,150 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/meshes/arm_a.fbx b/urdf/meshes/arm_a.fbx new file mode 100644 index 000000000..8f7c71b41 --- /dev/null +++ b/urdf/meshes/arm_a.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d72481ca801642e73fea847317225514516d10667d777c0287021a90c77befba +size 869948 diff --git a/urdf/meshes/arm_b.fbx b/urdf/meshes/arm_b.fbx new file mode 100644 index 000000000..59968f67e --- /dev/null +++ b/urdf/meshes/arm_b.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:27acf7d3fe472656b65d6185876002487b03a3aa6f26aea2bc2c49525dd5df3e +size 1128780 diff --git a/urdf/meshes/arm_c.fbx b/urdf/meshes/arm_c.fbx new file mode 100644 index 000000000..37c8aa17a --- /dev/null +++ b/urdf/meshes/arm_c.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:14f152298e48eb526c1d28c96d3342a8e9ae0e449a5824026bb9bc36c07a5a6e +size 885340 diff --git a/urdf/meshes/arm_d.fbx b/urdf/meshes/arm_d.fbx new file mode 100644 index 000000000..be192a925 --- /dev/null +++ b/urdf/meshes/arm_d.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d270eeab807b552603478d239cbe1d5dc0cc374c11cfd8aff8d49b78340df9d3 +size 316844 diff --git a/urdf/meshes/arm_e.fbx b/urdf/meshes/arm_e.fbx new file mode 100644 index 000000000..0eb040dcc --- /dev/null +++ b/urdf/meshes/arm_e.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8c9c68ef7fd225bc93d933c6429a4b1994cfe099c27a13eb4d2c502146e49331 +size 407180 diff --git a/urdf/meshes/arm_gripper.fbx b/urdf/meshes/arm_gripper.fbx new file mode 100644 index 000000000..2ed35c5a1 --- /dev/null +++ b/urdf/meshes/arm_gripper.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:32a03cd4a738329e8311b0f27199171ed6a850c333d618b33527c42f8a9aa990 +size 190156 diff --git a/urdf/meshes/bottle.fbx b/urdf/meshes/bottle.fbx new file mode 100644 index 000000000..5602ef9c2 --- /dev/null +++ b/urdf/meshes/bottle.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a6eda3d1f92892e01fcf2ad3dff4421d59ef1f4688fbe8e7e700151065a5b6be +size 17596 diff --git a/urdf/meshes/ground.fbx b/urdf/meshes/ground.fbx new file mode 100644 index 000000000..1e06dd797 --- /dev/null +++ b/urdf/meshes/ground.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:eb9df0b23202bdbaf6319b67e2e723197edec84fba09b53ee2c18ffbdf454626 +size 39708 diff --git a/urdf/meshes/groundflat.fbx b/urdf/meshes/groundflat.fbx new file mode 100644 index 000000000..aca631c50 --- /dev/null +++ b/urdf/meshes/groundflat.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5d1c9d3030e2c1a5688101b93b95c35cac89e10f2951d6844a9834f9ffc4f92b +size 18972 diff --git a/urdf/meshes/hammer.fbx b/urdf/meshes/hammer.fbx new file mode 100644 index 000000000..9f8b6b9e4 --- /dev/null +++ b/urdf/meshes/hammer.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b5e36777f78ac597bcd49076a0cf2b674d886c315b50b5d1ff75c7bcea3123a +size 16620 diff --git a/urdf/meshes/large_rock.fbx b/urdf/meshes/large_rock.fbx new file mode 100644 index 000000000..a4309c580 --- /dev/null +++ b/urdf/meshes/large_rock.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6d863fb8c715ce599cb6622d1951267ce0b912b1a4a3afd321a92df63653bd2b +size 336316 diff --git a/urdf/meshes/large_rock_decimated.fbx b/urdf/meshes/large_rock_decimated.fbx new file mode 100644 index 000000000..b85bf27ad --- /dev/null +++ b/urdf/meshes/large_rock_decimated.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:181c30a95da1588e80c778923c9708b74cc610a08489a73d5d5d622f6fa49d8f +size 67420 diff --git a/urdf/meshes/medium_rock.fbx b/urdf/meshes/medium_rock.fbx new file mode 100644 index 000000000..7ad1c3316 --- /dev/null +++ b/urdf/meshes/medium_rock.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4511a80fad809d60ecbd4a6336f83f3adf2e7276b051fb86224355c1043e0963 +size 317004 diff --git a/urdf/meshes/medium_rock_decimated.fbx b/urdf/meshes/medium_rock_decimated.fbx new file mode 100644 index 000000000..8cb15d59e --- /dev/null +++ b/urdf/meshes/medium_rock_decimated.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5415a186af45ffe67c4fd5980711f0c7e3ea91f8c8032b726715380053f906ca +size 76380 diff --git a/urdf/meshes/primitives/cube.fbx b/urdf/meshes/primitives/cube.fbx new file mode 100644 index 000000000..9326aa8ca --- /dev/null +++ b/urdf/meshes/primitives/cube.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3fc4c443d8ae8ca4cca8d98e6aa37a811db6fc23ce4eb169abc3314625a5f27a +size 11756 diff --git a/urdf/meshes/primitives/cylinder.fbx b/urdf/meshes/primitives/cylinder.fbx new file mode 100644 index 000000000..02941d306 --- /dev/null +++ b/urdf/meshes/primitives/cylinder.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4678acc34ec93779fa178d326f321b8cc67976c7e8df754c88fea29b34ee0239 +size 12460 diff --git a/urdf/meshes/primitives/sphere.fbx b/urdf/meshes/primitives/sphere.fbx new file mode 100644 index 000000000..07b9cab65 --- /dev/null +++ b/urdf/meshes/primitives/sphere.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b5eb2df5136ad610d75ba102a17a9bee8fbb0cb253a45cc6e8bad1c527559c25 +size 13948 diff --git a/urdf/meshes/rock.fbx b/urdf/meshes/rock.fbx new file mode 100644 index 000000000..0bd101bbe --- /dev/null +++ b/urdf/meshes/rock.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8098184699873dfc775269d78e3a2d069344d89b69fe67d38d06a3dba6f92c4d +size 18940 diff --git a/urdf/meshes/rover_chassis.fbx b/urdf/meshes/rover_chassis.fbx new file mode 100644 index 000000000..039d2bb40 --- /dev/null +++ b/urdf/meshes/rover_chassis.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fc1a2241d36784b9328ca074009915df5b9caf1926daf85086a4d72a9db8529c +size 1967532 diff --git a/urdf/meshes/rover_left_bogie.fbx b/urdf/meshes/rover_left_bogie.fbx new file mode 100644 index 000000000..a50ee8540 --- /dev/null +++ b/urdf/meshes/rover_left_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3c5ecacbf35e5a24e0f6530972d579c2171fd67f87fa727960630e2cd697e47e +size 1574524 diff --git a/urdf/meshes/rover_left_rocker.fbx b/urdf/meshes/rover_left_rocker.fbx new file mode 100644 index 000000000..c0af1b801 --- /dev/null +++ b/urdf/meshes/rover_left_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7da666d555f7b32538a016f6cceb1bb45f625f0b6ae15c0a43e0eea4c753c4e0 +size 1064220 diff --git a/urdf/meshes/rover_left_wheel.fbx b/urdf/meshes/rover_left_wheel.fbx new file mode 100644 index 000000000..ab6089917 --- /dev/null +++ b/urdf/meshes/rover_left_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:13129b734d971654de759255f9a6e65c2571bab60ad668a2c325f1a92b377612 +size 556396 diff --git a/urdf/meshes/rover_right_bogie.fbx b/urdf/meshes/rover_right_bogie.fbx new file mode 100644 index 000000000..3c161339f --- /dev/null +++ b/urdf/meshes/rover_right_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2329d7c9272175888328413b929e51a7896a103a701adea225281b1ecaebb80c +size 1572668 diff --git a/urdf/meshes/rover_right_rocker.fbx b/urdf/meshes/rover_right_rocker.fbx new file mode 100644 index 000000000..28cc1dc5d --- /dev/null +++ b/urdf/meshes/rover_right_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ce1307454a58a91b5bbf431cfa4c1bf289b62164595ed5c7d54b4af259f701b0 +size 1064460 diff --git a/urdf/meshes/rover_right_wheel.fbx b/urdf/meshes/rover_right_wheel.fbx new file mode 100644 index 000000000..bb5022926 --- /dev/null +++ b/urdf/meshes/rover_right_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:201aaec237d10542c6a97ee8afd7b7bca7c01b6463952872e22a3369b31fa3cb +size 550204 diff --git a/urdf/meshes/small_rock.fbx b/urdf/meshes/small_rock.fbx new file mode 100644 index 000000000..e141fd843 --- /dev/null +++ b/urdf/meshes/small_rock.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:22a7c07a5211765c501f6623ec9c7abf51705e392acf3848f89cc059526b1f63 +size 318620 diff --git a/urdf/meshes/small_rock_decimated.fbx b/urdf/meshes/small_rock_decimated.fbx new file mode 100644 index 000000000..c8c550bcc --- /dev/null +++ b/urdf/meshes/small_rock_decimated.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6fa0fc4a18f080c9abb31479f48da9174ad8f6c6f4315497413bbe686c0dc206 +size 76796 diff --git a/urdf/meshes/tag_0.fbx b/urdf/meshes/tag_0.fbx new file mode 100644 index 000000000..328f38944 --- /dev/null +++ b/urdf/meshes/tag_0.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ee8c37f9ab9725905679ef6a042378a22266302f18bfabfd4c1cf0914961e375 +size 18236 diff --git a/urdf/meshes/tag_1.fbx b/urdf/meshes/tag_1.fbx new file mode 100644 index 000000000..2a877464f --- /dev/null +++ b/urdf/meshes/tag_1.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:330cdfb6bd7338770028913dcc2ae1a48afe47b4a663de58b62d2e28b448b83d +size 18236 diff --git a/urdf/meshes/textured_ground.fbx b/urdf/meshes/textured_ground.fbx new file mode 100644 index 000000000..17ed3b134 --- /dev/null +++ b/urdf/meshes/textured_ground.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b2b267c0f5b26db4ba221031de72cc76e2dd7752bb978c685fd871371ee96c1 +size 13159980 diff --git a/urdf/meshes/textured_ground_decimated.fbx b/urdf/meshes/textured_ground_decimated.fbx new file mode 100644 index 000000000..0b295e00b --- /dev/null +++ b/urdf/meshes/textured_ground_decimated.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:935cce106bc7e4e258a4f07b3c7789f050a8dc8f537527911141328035730db8 +size 269324 diff --git a/urdf/rover/rover.urdf b/urdf/rover/rover.urdf new file mode 100644 index 000000000..8d2d6eef5 --- /dev/null +++ b/urdf/rover/rover.urdf @@ -0,0 +1,488 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/staging/bottle.blend b/urdf/staging/bottle.blend new file mode 100644 index 000000000..db12f4aff --- /dev/null +++ b/urdf/staging/bottle.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d4755b6ead5cec56f3b044ba4e48280fbddd3eefc481030ae56af4e32668c91b +size 933840 diff --git a/urdf/staging/ground.blend b/urdf/staging/ground.blend new file mode 100644 index 000000000..6f771f148 --- /dev/null +++ b/urdf/staging/ground.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:61158266eef1f35d53ae11a19fb2dff065f3b55cd379e136bd576615feb4721a +size 998428 diff --git a/urdf/staging/hammer.blend b/urdf/staging/hammer.blend new file mode 100644 index 000000000..9acb014c4 --- /dev/null +++ b/urdf/staging/hammer.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ce6398576246663ac1aeca8d03fe88fd9db0843b035bc6a63f85019b79645dee +size 908620 diff --git a/urdf/staging/primitives/cube.blend b/urdf/staging/primitives/cube.blend new file mode 100644 index 000000000..1433c642b --- /dev/null +++ b/urdf/staging/primitives/cube.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a09c7e9cb4693adba6f584b9b59529a92b5c9f187ced25a889b6f8d4bee31993 +size 882484 diff --git a/urdf/staging/primitives/cylinder.blend b/urdf/staging/primitives/cylinder.blend new file mode 100644 index 000000000..b9494594e --- /dev/null +++ b/urdf/staging/primitives/cylinder.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bb61d2ba5b0e89acbde4455e69751a51133276db0d7bef3651119368b90d29ca +size 886544 diff --git a/urdf/staging/primitives/sphere.blend b/urdf/staging/primitives/sphere.blend new file mode 100644 index 000000000..ecd3fa669 --- /dev/null +++ b/urdf/staging/primitives/sphere.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:26f4ea41ada2d7dbd3a880551ef82c0d240add487fe610ca7459195a1121d506 +size 889868 diff --git a/urdf/staging/rock.blend b/urdf/staging/rock.blend new file mode 100644 index 000000000..225b42619 --- /dev/null +++ b/urdf/staging/rock.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:58f34c806bf5938d1da0956943cacbf68998e596d90579e72f06cd845c0ceb29 +size 893320 diff --git a/urdf/staging/rover.blend b/urdf/staging/rover.blend new file mode 100644 index 000000000..bd604d226 --- /dev/null +++ b/urdf/staging/rover.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:eb496230d3a997f741fabaf401e493432ad6ea09292c5d011edd14b1118b0490 +size 24983740 diff --git a/urdf/staging/tag.blend b/urdf/staging/tag.blend new file mode 100644 index 000000000..04292f1cd --- /dev/null +++ b/urdf/staging/tag.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:152d65ea87066650aa5882b1985b993f1bb2f0ba417eb4a7c129c20a2458264d +size 898808 diff --git a/urdf/staging/zed.blend b/urdf/staging/zed.blend new file mode 100644 index 000000000..93eac6282 --- /dev/null +++ b/urdf/staging/zed.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:808955b4fc7802d2c7c40f400fbff598cff4777465aeed4255e2c7959f748fd5 +size 931268 diff --git a/urdf/textures/aluminum_base.png b/urdf/textures/aluminum_base.png new file mode 100644 index 000000000..9163aa9b8 --- /dev/null +++ b/urdf/textures/aluminum_base.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:13b7f63c4bf9328869dcfe1e346ddf5375de51dcf078ba385fdb222267a890d4 +size 582262 diff --git a/urdf/textures/aruco_4x4_0.png b/urdf/textures/aruco_4x4_0.png new file mode 100644 index 000000000..0495d2249 --- /dev/null +++ b/urdf/textures/aruco_4x4_0.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5810218d0ce8036dabca15d6df95b73fb89da32eeaedbc1cd9e422ece80bef9d +size 29914 diff --git a/urdf/textures/aruco_4x4_1.png b/urdf/textures/aruco_4x4_1.png new file mode 100644 index 000000000..3f6409389 --- /dev/null +++ b/urdf/textures/aruco_4x4_1.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6c8b2e6554ef695f04d2d6ec099e42220f5aa8837b5357389671bd1af3702708 +size 25967 diff --git a/urdf/textures/clay-rock.jpg b/urdf/textures/clay-rock.jpg new file mode 100644 index 0000000000000000000000000000000000000000..54c5dc8df530c8311fc6fbd31189494e0e50e65c GIT binary patch literal 22869 zcmb4qRa6|l6Yk>f4vR1D?k>ByJH-}?yHjX^;_L#8OOfL4Zl$<8lmey2-K9wB@Am(2 z&$+L6@|NU$nUj-bCiBhwxAxyJ0HM07nkoPZ2?_AO2Ka9Spaj6iz`(>n$Hv6O#KFeK z!KWm|$HT*?B_k)IWTs~UG0`(IvT}*>v2qA=Ff#Eg2nb6^$;!&I@G7e*NkhbBWTpNG zf`pBOgO7&~BqRh%u`{tt{eR1U0{|jSBrK#P6eK19G7%CA5z>Fd0BQgL<)5qq&2H9#Ul zW)vV+(6jYMVft?!fQ#~PG7$<9Ko0OMWvB6kydT`LMSsu=Y%wnnQn^jl(-u2SHlJz6 zy1Kdq<&M_$*=-3*fJ0tCur;Xco zj2)Y!QBET`f()GpPznf&_BD+ra3@m5EEhQA240M9BtA~R%cZI(MTyq$g9uMW&hTu) zwaI)TW;F3JYjo*}r$es7K23V4u}VZ=u)67?N@U~9BPM!>NgvFY_xVGef9p3l8s3~7 zIF$(s?k4Y#Fo>Rd#8w+8>v^2 zA$Khk9$1m%kj-#GfR1+p652tzhcHs!9ts^Gfg}4tB#~yP<()#y`Q5e|J;3TO&2(JQ;^I9I8Ad-fl9BV9%}C4wY{OE#nWak zD6BFG@uAIHDk{wkr1rN-^%`qa^W{SlVQO(Y2>xr7e@(~b&7R#G(5ULqsjk`;Hm7i| zKQ>>aiTg>p&^x#zv~aPcKzx16B&r#6tx>5fdz~p!ec*4$i_HS$r` z>J6OAgMpW3yd;lzW!zE4sVL7ZqnlLx+2CS%jzn>QiC=Q_!n;7;kG0P9X(Jg`Xy^BE zFXqwLnmsfali?WuybI;YG7(=_;Y^O{8^GTJVl>4o1}|B6xBa;N~!=p1CdqXjkp^w2*4B5E~rXrvb$7v%1fg1H_0BX%0YDx(Ane9(=vQQ@FK z+%iZF^rD2MU(!~( zCN&E|<_g1TJJsA{eHGCO3T_D%O4!jt=C*p|#>Kk?Vh3Z#afGSg(v(a2A`?P-Q7s>A ztFe9c(f;FsbakB2z}q)bj(YzA8q70%O;?p@o5Ce?aq_~U)Y>uFf)bws>c__+_F6be zDzOh$`wSD6aPzBb#hWqpNkI;fK=Vh~t-D2py4|)2hqDExuH*s-2TioJRaE2HJT>MX zY@!*<>^XDZPF2Hio$e7xAj@vd;DCD613Hz zDF=ZG}yAEuxC0MNf7d*^lG?uYLt7(i zmo7TYT7(;I6KOWN1<)@d{5={_j+9wqCQ$?sm!5`M*?o?u zf2c7`kI9ji{hokDC!WS4Ygcn5h3#K=CuY0@d1Ed#(RB$H$D-d*IKah+V(5pg^2M3` zP!O^Tl7@I;5TiY@{Ji)}alfx=EZpGP>Od7)5VVv#l5jb?pRPd>n^)sIX*l`monQ6m z-q!1PtOe+AVZjdi{MU^Km`Cpt?!=14i(A_R3v6!&wfQAUj^D6p2euK$^Re)64%$g# zIoL+Zyt>MI;%9e7QW?w8CvzU(y$mfIm!F~s#h5F?F+W z`g1vt|EZSOq0otWvg6*&=vcWdFw9GwZ*TCHg|mz5MNiR2Ci}Me>%};VoN$6oH4j&^UIx#oP85zs{U2NWer9w%&I3Nm>>Pe3c?A3)sn(yo1z{Vu7 z++W3Mp(Ct;zjqh-@bqATeS`d0PVKg`mKJhB+YbEFj|JaJu{?ng;}wYJpXe!q&|Hy@ zbXmtv)2~#7qdksYtT7TLx)gD0=i~4F*;*&^2_mR|f)0;3UD4m=y)96PwVMimGWfWj z&obC3piNn^m2^K06u+{B z?b-N(e%EV`*siJ?`ZMP%3w#)AROT!_6I#t1W;BTrK9uVJ4`7ZE7m2LA+HnSL9j!F-aiRG@CYu@~xhu35gCU0ZP^W41Za$SRcaP zCIrmX%cF~Q?1*S!9=rh`ynOCiO?_(+e$xMz667O1-1}y^N-h1^i?r}`W3{`CsI|2z zhQ=8<6QZ&To;B)X4|QMv?0t|c%J8x4NUV5h?5$}5;{-$H2(}WcdNJlAXfxe#U_$P` zkBA#aAw^V4G+By8I8DxdG{?%d+Itv5uEARroCYhnTg%Zv*`EO3E@nw$3ZIpBu+X`X zjQ%>}N?-cabX-`+lT(^P!psg#M@qnAOmG?_XJpHBND1w%#&wyGC>4;H4aUYcBX?If zwuz54^Rq8(P`{~+76Fr2ct_5pj-47h4d8<{Lk|1Um8uA+aODX9wy`mFQt-zU^_I9H zbt9(VEvQfL`=w$YB62C0t1y_EFY$ZM2+jixh)xaD!u+*(-NqEd{ul7 z6i7xd5Fg7_t6(IdUB*o_>2G@*W~)H$SGj4Tf6cly6h8%rMG;b*u2%i34bS~leKP2a z$inW=yRR6tv)5zkHBdwJ!X;qzbNMdUhVHP8gP|PZOmwB;PD(a}xZJs!`L{@if~$>F z117?ekje%tvMywSF9dF@w5{!{6k>EN-m;Z;H^r5+?FZVMR~!vH`diUX`VbA^wKtr( zKs#hLXXILmyqO-g{z75&?z^X5hgYLY<5Q_^D#l~&OQ=muPCbEGN{XONH1X(XWF=}3 zA7>D$!IwLBI(db@ z5VQiTB z-4kVg&O0O9!3^TCg{eO1ddxMuMy;0nJ9OOkRG{GV$KUC9B1gQ|!z?S3*Nm!T*yIzT zfzolb^2PrFwBO4nun@=ic`{OBl!Z8l1BIVVvZ^XAM(nWm?QKeI@R4YnApe{{N=u}! z0TA}=r)uf|m@7^u8)T`E`|Gg#e!$AqOacUK`oN6>ObO0qKS0nz4QIjA=PGE$4C zI0W3CZ~8EYt{`JMg^QHeul$isY7x5`^7WI?L2q(f&?pKZv}{%|cz3^;@C|`<<%ifOr@i#RC*_<4xesoaX{|6u&jwlXiOrJjFUc`%?*PCMR zNaOU*UxxOf;6R|6Hzz6y;xXoL#Q}k1$7W9YXaf8JMhyGecfTunx1=^4+SOPXXDJj$ zizF`R^}uo?79+yyFx^zkqie|R_gco zlZrmPr{jfxSA(W-K-!3l-m{`)hP=%garR4MT>hwyb<@x%(F$VF9w`1KVEgHyl@38H zzLzfH-f#Pt!}|w!iMF2*(t$MGxHdX8LXmxK*F~+Rqw_^ei(Zp`W2-qns4M`7oEaX- z13QRUP@caasJ4G^*UiwCAm21i&pZWhzbnC!kVpZ&d0B0Pc7OWhApbJf2TT|h$mB}f z$o}0xOZ17HjX6tN4-8Dr6kqO~tE?g|wRec40->$ZhY(Xvj^soh9?~R~#13hS2~E+m z+b1ZZ$0}c_>wYX{U)~_;{E2G9oJWZf036f&mby;5!98%HBwFptgo|wAkzGPa6;w|) zfKrK)&#|+um7;-fQJB^Ksvs&d(soDTiOu+1b-$S{gq1rGeZbCLRt+On^7mb*(rp&j+-cwKG!B=qH zY`MU_pEI1_3^btEC{yV{q?3_Ny3|C=PROoc>4cNq8)EX&g7cXB7Mh2&nue6oGNwd) zh*cBzVdNal2*Jn>$67BwOcbeWh_@J{?Ln$Qm*+kfxw&<}6^2jJY)e+!iEWNQ7$m#l_ z{g&u}c2riM%Y5AO2ilA24)`l4y}>)<4&>EUw%iB1$r6JUeuAQtuo|q2-cGlMShnbO ziA5j;bscLh#PQi6?^Nn>E%gDj32*b5klD6|x({E%u=(*aW`iqK>rR`{X2@+wGb{r7 ztY`Vi7oF$Ofx`(0ksD9ZSJftd`0j}pz4E!kSlc3eJ&Zr#c}eK}LxSA$ADmHp0kC*i zkgC})I}qdP59~qsYzr;A%b?Tm;JJ)P)L1`uOs~u zEr$bwl;S;yfLV$Xxj(-fFk(r%>Da=@o>U8|_Y1>V)&`kgG*icXDFMfFNb0)~!}WY6 zG88Z2JLD96(uE`@X~<^(5{No@_YbeFvh%l`lx=W%@Zo9{JR~E|Fa6s*k{lzv@wb!f_bH0-mN~jsJTG)ayCKy0&lhv$v=M(o`D&X`|Gaz;V8J(zJ;lQl6+GXm^T(Xm788a=}|fCSDr^~E|o+m=PqCltclf; zUTF=zg+(GwjK8|#d4SsC8EYWsfAOAsnU@tC0Oo)L11{OPFt%U0qBDaB0r&3)EJSdZ z3=#^WHU%PFg+=4pVdE5G_cbpCfa@cyoTcm6?t8OGbg9Ley>av|kc=szfG@3mCwde{ z3(AnLjvz!8U;yqyPQgt38j?g2)4nvzZAhxAMn>*knKGP@UzM4}mMWn#?=waKyRv$7 zI-&X(ytsPXNO#C>l&wNqc%_;S*|pAHtG>C4Ei;j|)uIf4h7%IE8!jE0jN{YPyo+oK z)w@}!VR8hl+4E$9l=@+S;+%QE&3dzX+lBndQNx{1Z5r+;nf0o81i?DP&lF~f&z)eW zYnRW7{F8g`XHG*mEcHwRRk&+3K>Su#UHP{Rzwu_YMEyIOVSRhC?S~R~167}|3-JQ% zqfI-ufI>@srtJiJ9u@@VE~gR(BuKyGX>|L(cB`5$io}1@ZcWojFiQBL3QOC}i<`I^ zpDOF`I~l1aDof^76r#&NO%t#qqcY@Q78IrZ+v-Tt?8by_McAn;H) zC9!HqntBw951q;nAnax|I5#9KlW{}^w{sN#psOU-3-mp`(g$n*=mY0?Y#j3rKNQ~1 zu)9@In~Pn3Qg`C>VMJ4nv4u)yw4?^P%rU53ja(I5dtKY`KLG!w`Bg}>*~IE%0H|K@Yc=y1tp62}0oWbti4h#^UGH=dGLV9TqE0 znsXW{*Q5Tn`c(AmmnTP)=BxOEEtB~y5^)vg>)qT2s7~fYMZyRCR6V9HV}V9c(q2IQ zoKwF52r&!eS;kW<+&avBr?+sal=FL~$$zQ!5uK+))6=YIH(D(2&z;3;g%1 z(|fiL99k#`UuQW~7wQmxuXRHgd$?g3n%F0d){Q=qiqd=7SiY}~H=s`fx81(h*5-PW zWYk6kIJEJz_eI2{9@KCtx8da&(r`^i_?}i!!P=)^cMfNqs=cDOWI`BV1=z|=<(W#9w8Wc9)%(+9(!~P42sUs{{sYhxJHt8 z>vp(u$n%H80PlNe{QvS8W&U8~or-p*pQ4fp^IlKFa-a)7AOS*6wZtFDx~SUro0Axd z6=~zYV~%BQ!FhHg*-Sqt{Lp@eTH@ZX7v-#L>6fIC2Fq{eR;vvS_v>o|?(588v*yKH zn^Nzu^LpqC(nE}?uBt7h6Fn2O!UMjg8gi`p$+juiWiecY5GiyY5Ryc`%e_$IP5H3Z z-JOnZJ7=qwWx6bJK9^FX%7WdJI-(RUJ@J)H^FYKXNo)veBWIq2Q_2n5j0h)?ywFz_ zR2(S-eAplrBjb!h;;*0))68`oRxBDeQN_r{Un%hLEC~p5FE#e_I+!MNUs>~5~N!GJ7oVQ5a^h9LK^p!v2o7;RL=#i6Q9PrsI}qY zG8iw0$3z(MJxEil(Pft8sTV4WC8pF~z2k>!V7~C-&-Y zP9&I}F-gYF50-gehO$6Y(OA?7uvX>{tX7KBu5B^)wHHRzY++nEbs5*CG?Y2mhnZ0yQ%L>%wyQu7U)UAQy7Ff zXAHeToVoJ-2PpgEv}H|@Hr%VvVcV9jIbVO)@TpK$;>5Lfn`M1$@&LRV@NR*<2hxo!NL?)eyOD zFva5Z{_|2FyGOA44G|uEzJ0%{8$Ln%v!q4zH@=ZUVWSc)|icL zfZvOejG=8YU;PIl=<4dsLl@wFOSbfQPaFAsD~z}Em)X~sDUqTb(%9AMDI|9l(L~p} zA#q{jfrGWU={eY~h)pu#?~hSfCVDztdyXU~*(NclT1%DO2pp@Uksew%x;A66)FB?V zd-?=CHw0Zvv-)M3lavZD8p*8PRI<8qJt=8;lHx)*(*y>d@%!(#fnq+~z9YiLe7rf! znEzn@wnZ+$;%V-n4UdjeP!Ch8Eg{JK5IK z&RjP1fJLMUL#uSZ9#X^*z{&kv%<0W9`{UpB$`6?v(QkY{iAJRE1wQM8uy4@eIka;K z9e#gJ|zHli#vf^6DRq^71HaDJ{O9q7x(}H3-;clAp?zTQ-NOb$OVVGI^igKGbMS=Yrry3X2jS+XW^+RYw{N z^6Qqkcl`&*)oec^(la(f-Ghk!ESD-&Khtc&Ahz@_gT{<+LQ^UWmZ}L&$rMGIZT}h3 z3C8b8s{yw*?B!(Q%fEoYMrH>0H)+2Wn%wuP2L1_-!s(r1NDbOCgK>9r=ug&GG>L=bLRU}%J#E4;W3jKn$N%Q>o8iI zQmk)E-BixqSN1ABqz#^%4reu92Q1bmX`rkj^#L8-eayU;DyKvj zfiDK=*>*!DS#)m3$|hR^JDJbFKK{~l#3SL%p$wdO@?Eb7Uvrcascq{GmQcOzoXe^j zK;o6w2f z0{U%H?_oA2fIcy;t)Kt%GV$N_N5dIviza*2-dU(!)f%rvhew9re}I;L9G;T4!|07n zWyu4D(r^sfL4>P2Pg(BznuQ>5N=k+`^9Lf2*V7MAv#(U{$ER{s1-Hrv!|NL)*;{XU zqzflM+TNGHd2~Mhn~FpM`@RZ#w7$EKd+gh9yK1L`aoe%)oS4=JN}M?J zy_k^OqS(+d@ZC;NRu~ZjGo3Ab}awy3|XRD-L2|rLg zhi(Nto@$>{&EGFwoks=b1Jy!6Rwk&JXx4aj3tk;O{Xi zWkoye)1H~IqpAY(zlrqKOprsj=#eXL@qr)zQ&QV7qGNYt0Wojv?~g6 zZ@(X}t6vx38H*pCn^tW)f`=g;IExWk{yYx8codD>US%1}zT*zLT+Hxx*hz!%Fy<4B z{+Jx=CK#$bf8~7BtK7_=j)-PculkiPEwak#+^;JxJbc~h1-HMCAK;g$cI z8+*VU%eL)lXa0c;^S^;LiqG{@jXsp#@{;;*%vPSNC~|Eax}QKoZP0E>@RvT)4sl*inj_*kb0!d2)d-4x0{Ab29PGkk>gZ0kc-IaUUH6@y{OyRaB`CK09Si3aKvJA8L0uU3^yAJ;WLcS$_o3(yF-`Pz^ zV}r1Q%pe{%7%p^_j#d5xEN$`Zc+~N1eY)7RFdf(K)GiA7wg`rj20y;@i}!`w;7Z(Y zfEM9D6H|?GG$06k+_b6M!hNGxSMw=RxwRUnG^3T_-%;oBpuyyEyJ`zA0z9}e0d@>y zKBpmUJmlBy^V0gF(+@sr&*ASkV0?-fVf_$P;0#`~Irl*jMKV{@tyJc~nb6d+N}c@J z+sbAg&gldTRvaT;kZdP?ogUgstG;cqYiKfvt>}Kt+)j%o28SQFM@ktV+!5mV2N!`v zh1xh||J3r=Ng?S}ceQeYUibP}E>#7-0?=oNohl8lsw!X)4Xv7{L#PxS zh0Yvsv*#}^!wFW|uFb_|Ct+r@$ovz{K`#CyT^0X7Krfq57#H0KlCEnrIs%;Q*vFtG zmTuFpia(=;dI~4Nbq2_f@N}n7P7Py4m6pRAD& z|9lDMOd0jL&vqyQ57lg`Toxt;cFfB9-{;g52i=Np> zleXA?6%i3+3wg*%ezxJv0UJRNus6<64I~r&ogF+mgR^bgF{~_K(SZ83-lUkv!!%sJ z+FfJ5mZzlwq<$=ApQh0dbq5chT)28H z`)zIw91PuQ?`PR*>Gz1}NJW!#z+^a@IPnoEG=Ik0&Ay}tRcap{?#pIZPYa&t5axaT z3z*r_+hH9_o%JJ@`_f_lWyt-!0M!30ow(9L5tA_t95NI8U^l2kv^%vO*Bv@l7Z2CD zIRq?88{arxM3!VEYwzKbnCE!?qu}2x21rY=C=OBDZZwt&J7lBhnjxP-xRTvPqGD80 z)emErQ^EHY=8wg-sUQP2??%(-6WBd2bNnRw_2H%s?~ z&s7~OIeIP0r#7kD&9pjl_WZKBub;8+g|ep_&CbEU-3SZst?E+g+Y*vgXQ-J-Fc)CH zI;RZuGvjCZq?IF~X%qF$_^6G=j*aCybhPQ@YR)zOv7+!h2^5-m)ymliYIP9V+KE}@Z=I8 z=*%#nqj5e1-R5nwxnY(jjHwUKgm_r4$MulYsS}^AaxHTAC!W?mQ!D*r3a31T=xV>d z7;Db?zTH_-vc%1RecY#?4l&r{0U;P@M8EGB9CU;p=UlpDu1R~h!ePl7Nu&w23TS&O zWTfSSlt5VWxxrXZbriKcpv9m*$0k{;Khl49>amY2FT}DXN*6&(DWi=K_Sh-!z9xtZ zFIHjF6r)tgko^HSAsBUfF`XCX7o&M;kQ8fGsDgFxNpBKl%^REBeyE=WX#|VksQ2zB zu)2RC5+5Mbiwrgt_ED@2WJzl5x38@ne?ai<29E*n@S-jLF?rN@-UH0m8M=fRhto?1{-zNVT;mA+YwB}TRjPSh z@?&kDcb31?PJ?irz-db88<{d3H>y`7#W;S@sdIYyzd0hDaeH7mFp|^-d*fHYe*h7h zAf62YSJwpw_q*c(&pOPxDA-TwWDlfGPJlMW&m5jt&J`S4YA)6o6-s@3W7IH?0MS$h z;HZ`sciqS6$zMJbvjRpv8l)j*oCYW}glsAhh5ha7tKB8a0nWf@!=xdzHCTs0zw@Bo zm_Q>}ssTnmTYL&_O1NO)PZ16$J`Ys>-uh%FTq0Z>3g0EFLo>l3wxst9Q-!oWq5%>E zO|79V$-D1M7CTb#ktw1-b#luCGN?Azt5rD&hi&svbZLmpKQ-#EJ-WxnDl@jXC$T70 zsKe0y8O*a?b8o1AwiKZ}Q|=O@VneE$l=D`FV5D%WffVeP)Z-z>_AwjZF3kBy#InqZ z9%*+}vcR`#4!s0F6KbZkz=01vs^Y5$Uu+O~v_xzu~l{-ZH zeXK+@9`>=)?AsAt(*&|1H~0FE_3{lt5iHj<4w-sI3~3g)w^BaE8dabwx#XZIU0Jl$ z_MqTf&7m(v=hj-noD0$8hHsJrXaX8iu8y{DP=W>3h-Fs?+B4@R_)mn4>4{viaDv8D zDHme#1F|(K={hMnxyT<*ua6s#hqFNn!!ClI44JYeZ5Yq`T|2$7_3lU|$Hk_GFD}y5 z`pj(>4TSl!#*K<+U4MD4x0;t8S{@1(&o(Uo&bc_sIS!+#O4`Hwjz96-=Ogaf2Ai>x zwzG<>!!iD)W@JZV?%zhlfP6&~U3c}B{#ka`JzA~WZHQbjfk#H!SgwU(SG!SwzJG0! z{BO!bvuRHgFKQ9)nYyNfF?Hk8eL^%E33dhC59YaSkF5m$-Xj?Av00-rmT%=2la!+x zz9W&4{0C?k-aXb9XwYybUA@G;=0@D(P38Mvlez-!i>pQ{K-ecFwWH|88v!#onhP35 z%uuTP8B`w$SA}XvVr0yPX-~7INB6;|1sdNKD!i24<-iJ@zF;$EPB~&2woUIZ)g{ql zCd+&ou&rJpZ20Q5W{gPKt;o%=Ftc^{>Yb~R(OY65tquLTS=0N4pt-mLkSq~<+h)As zCBt!)EEMd)>zJbRj(Lb~p8+RJwpP&Z<-Ollg-ghGhBPr{?c;(HT_IQhz@e1m=ey4z zNUkxJa0qZZugTmKsgzwTmZXmF>ll5`20e)MFOI;071>1y;^nqhFO=F`?{S?HE6V zv)#$p-w18SWaGvy+Zy_A-DMF*_P+v8(aE*Da}gtM%(kEAJ#?!j|g24~5-P z#U`Vx*Bz~E+qG?5s%pVX8BKp&-x^cuQhkI3Hjf!TMm$Vo$ zlB-D(a4&;lyV#nmJ4*Ooe?7U5JW%@{GxZDuZWDLPhWjl2C0}lPx=2L`=?ly8X~CTa zwh9|f7%E8x&D-$VX&k~|BrxZV+uHQmrLw`w?@)cR{0O|u%_TwZg|~J_;7nDco0nY6 zmXAWh-5G{L#A29$1Lyd+w-F3pWHEk`Uf`$dO#egpqmQL{bo3vXXv~Hn@elOPO81T7 z3L`P~d0DTirX=p0H%i%ciPkt8II6@Dvh37#Lx!=&zjt3b5CViZhf@X0A-}5x4_Z45 zA+{u%v6z!HO_vCGA7_oL##9ip{6M=vESk|5^APnbTBP*JfpEa6&T9J7AO1l~la4n6JjKNO&&(~kJr;cYO(g(HDo+)};oBRWykfk2DQDVdH_HAN1b zQ!p;W(7@bMIMDi+T!Li43|Gj|-tvaM!#;|e9Uyo5d?4Jk`aYkd5C@xseBDbBG9fXh zXQcbRV>${@f&xu_-&ZL;J{A?-<_2*yAsUKIX0W3C`K6Cv+MS4Kdjs`jbq-Z)MXK(g z@^YtCr0;#bcYb^@z53xE^M}xHMuWwmC1iXYzZp_k(*=b9(Y*ai`WdPZ*LrO=2>52f z6slWI8gF+~>9B_+zONqbugtVqld7Ic&vAaPz(%19BULq?kRk+Y&LTrb zwafIm?m7Mk7}8Lx;{FqG^IaeVK3e-h&)R~!K64|ocCL$7Rjv;NN&_Gboa`ANa!FGp zJwMWX`6K+rHttWD;~B{K%@-lVN4dXL8|FH|zifUUW)O!MLd8O#59%*iOxxtsi3jd$ z7mzbFVcWb(9}L~#r4OfbU*!%*!#F#lxNHb%vhsY-&;ISQN4-TV+OG8VECI_*AGvVrz&i1w!64c^;A4TO&330$+^CJ^Hv$}tO{*dUfK6FihqQ2w%muMTx#28bM7Z$-9; z`u;83&!W4U)ldT>i=exYm5wM(oxgdXRr-AX}|FOj;l9Y|nRO4s?)MvTE(jypGU>V9tJ?+Iasq-*a0I1J@apU?qn@ z1TtSWo?aReW-b)q%1e2c=3kpX(>s_CWu^26d+ph`Ki*Z2Tt2(cxAg?mp%kEqW#b=( z^(-Cqn&u#_o1xdqm{(-1+7?x$*s#C7b-6rF_-gd`v)hH?Aw;S^U;f>qwDw<2250w# zwr>}^Q78{lH>mikjaHWeZU^p;YDr8e&^$tKSfG^gesbhys69p2UuK?59H-9! zkawyWo6fYW503iPPn2u648A`~!z_=#@S|e26YZrJbW~*^4>KMLlu?9DgzFt&mvtK2 zBtW-Uc3{f2Pc}$Lu63v$1Zd+7f1W_eAv5c&Oa*J5TyT7R!*D!Yvov|C)v>Ii zQxoZw@j)_#2**$0W?)%%TV(BXh#MzklC4~Z3BVsCpFEQi6{UYKKib#)Gd2uBQ)rH- zL7l4hmF6&y{#rpnQsnieDEkl4r5utDjSJ!HFtEU0Y(cIjgn}YqAxLHcM5PlvYiYP= z;EZN;NxRk^lWmv4xh!U!Mlv=>KkV8^2ewNOp;I+6qEYSgr4w{H&Rr^O2)1X6>PAC3 zyR4W7kgNak2G2T!;mN`(lQ7ePW*6o5vx?B{*|;9_*rybqcSx(*%1M(SApue-{TyZ* z2H33BGRgwBGd=SOt{SPkB%dQXQUq!3eK%-~+@g3}8A5#4)9nhJjpDsZynJDW*6JKN z=klQ#I=!VHGAhO#TK0xILiWxl%Vn8dP9<9QE#W|DW?&`jRS!+3vlmi&@Xbj31Dj_E zYrGyRz!^+s*=1J>MaZp}*1Nh=+EWOs_A2b-Rc1;Y;c;4Y4wBFTta`)$`P6fL1!{F= zifftVQ+&|lBWLVU(@Y6;@glrf^J7qbz}jlZ#BC=v1o*tdxyA`*rQ9E1ba#LB0&x#V zucgiARH?^(^QmBoW#FjHp3f_X%?%s=(Dn+v{7HI6_+*a?+||F`pQ8<{Vn??0#uuR* zDkIHF<2HfNeAwz*-I4n4!Cfj8Pq#xV;Rtgu3pt--uB>tQaBtXsOyr8Yps|TzMI{KK zau8f^PmM@9F=hq?Rw=tzc@)DiR>~ne0Q5WAwwPQ4r6DwP4`#mXNc64r7O%QFH#g*@ zHgQB-drKA8D%KnBlBNa@oXT)7wfZiBg1ah4M{caIN3IE`ZmOBqlgYDdl;RjJp~h$9 z3e?#JjzzWGyfRu$8CuF84YPNXaeuiU2{jgro}#{gvr(@tq$|ASormp5`>|^Og7a%N zYeu7bmG7K~(OGs23^{NHq6zcDuk}3(J6;$PehFI<=UJ>9N|qvDMxhrIy)@9E7*?K& zY@pA16I_Egb~ygB8V9$7i5a^y{;TL>ZxZxMCu2AS`O3UjUbJzCCxG*bO5!sT_J?q7 z?$>*&m?h|(KeXxZXFD;r(SKQ@O?F9D%&aE~O#l5-(x-1ODmf_fqPB?~jD6<}jFnaa zz~dxlnAE5h^34zgvQZ|j0?BBTk>7956SfrE3JPd;fE>g$(Wz!J9xN$9^|6gq{KVBZ zxdra>cB7sSHEVgGZZ{V9>gR20-< zcxUYQTk#j=hDi}y2DMtyLOsuh@uzl$SxXpQ#a(5n(nI3V*)3jP+x;mmdz|mV#aPy2 z&DBf5bpv%yC$HPtEv}D^CnFv7?yj!m3df1Z&O$1P5dj3EQF(teF)%-|`u0t&mrjnj z`ReF%0U!u!*;1SPv1_vm_$p-#)1a9=tDW?_sMW^2Cq1=gj6>`@pvg-|+}C&RN^x&# z;<^}2xlF$rKK?Lg)%5~=k6Ag$Q|30p@u3`XOiaT<$}`*Zdr~0NumW9&lYRUPyr~mf zSlUF}mW|BIzXL$(HMa+wJMyS7`mhR>ZIx0ee;v#5s040*E17}rC`yl@-%C_o7IznnltCU-5z$d%S-#12vN$?)DumN+4x3@kjK3gu`gO}Z{d<$85zMy|9)2Oj3quuNEGx#U8&=oc$x zW6cizOy2N}HyNZmZLEp!*Y!G89EeZsLT5vk7SymxkL3~<`~Clzh(5VNr8uNd&BumI z3ZojR;uFs5j!$*QFs7~zS}5(aXi%agz})qK4VXng}YtgbGQLj803*?RS%l= z>ppoy)YXSf2dL2xJ3_4EGoo$YQ*Do>E+XYmY}Hc7TXt;*dx^0|m+LYmyq%aBcwRM- zcnoZ+J^IzXs~O+lR#kx0uHWm#5(j`vEGnEIzxmPAsSF;-cx2upRw+R=xYDklI&@gItHykebjwB_Qe;+Z7RA4wV0b z7nwqS9&^n2%!kGS7%qk*2HuUpIrHCygqlufw;c)zpbpm@5n)H1B`Kb_tEC|e(+TgV z)2+Y=;r&82cbg-FQr3XE59XFjV2ba9bsR^P{OosA-uRZJ&8eK~f@^s6ipm(j_c&MYZep{uj!yDV&d=T)4SvK%Ih;EiMBF`QfCM z+{Cj6%<79X6*y}~2I~IcjLH7tZ(@c8tUJs06AgMo7#Z(1+*BV=-I=cGPhlqs*8-cXMVrO%q?3SdAhKL+6S-;$bGAR z2xrPF-_@>{8cBb(!7B5Y7(!`BdUeK;?Cq6tfD9~x1-TZv(x<9;190)DPMmc>;Wf_G zZNW(r$?7)6opDk=x>%bZ`+(f-lfOt}Nl)1!y%tbcfYSTejMMjpn);oO2Ijv?w|FU5G*;2%g7BmgSU5oP9evNl)M-Wz*Sf8 zLEbXvSYJ{X=|wRD2K?pnf}jS}n0-x>eLnySBQ4l`f8`H*1R-}ljrf}gvJ8K`k(Fuv zab(r-#!S&6bhgkic0AX7cT8y&%|nQF-=(!6T+;jh1i2VS=L4$-$r(|w74%_acz8`J zi4bI#8P)4ZJ{r7a*32Vjz*QQue@fK2&6@ex{4(P1MO$dyPD>VZu=hO$eH2h7wWYY3 zM3W+9jmT44ajZ7+tPudHPUrC&RKhGAub+_h6tD1sB$LP^idgg=+ghBjs3MtQG1rdMb&PofRWyDhmy?^rzw3oCW}cf$AtAJV$0D zDh}TCa$^hzR1@a*pu040r%w=Nd1?EUo7HBsj#H!+C(JqT%QeO;9y68FJjedJ*R{*= zuB9-n3k>ui?V2iO_^rx|Ux*4@fWWz5x~HG{so}bYW2!mu2bo;%uBWlB$iQ1hV;UUu z(l+rd!#BCg6B|O0IHvE)v4h7EKVeuQB0D?!~(?a`3kj4M5MDHF57G?DBjyW z(H{)Uz6-|sPrf--5tejqZb(ywIL5Ku)~x9m(ldep2#j)ix93fcAXqMN8~RWt5NiIl zqh|}KX-q>TWlvheMaJ)L@*QBLgB$(KbfJ})jcWRFxo=8$+GV(i7?YnU+dkBmF7D`r zaY)AJ82Zu~x6Zcrd{~i4*_eE~y+P=GD!6t`jamWb!*QDJ#iGp#2@)J^$Xm+$az6DR zhQTuGvS3Vck|I1zogam>)lXj3MOCcoZkvecSpf>U#%V&<;n=KhrV@FV^H9WW=A1=# zp=Cp{lgh`oJBp(E@8Ba3Z4L9(+ z%ok5_Cl*;UdJ|VztdmMzkJNe(M^b2XM)D99nTI|0 z6xd_Xs`5XSZ@wwqSo4H}Jw)j}H_ksgmTREOh!u++i32(I6mto#STll=WaC#dC_X1g z*h0ftUzm@V+K!vbC>jZh`Bq&XW)00xKx&mC$#Ov_@PUE()X*Rh$HPTMajA&r&D_%i ztV2`2fOg5KU%*{T#XyfL;j_OWM}K;Syj`y^#igyhk~DkD zEO?A_$ohX;dOg&FNVNF@(pWIT^dr)+>Gn~)lm!yTfyd$86X}|V#jjdO5n;%?bM{D} zonHQ)^{iuu3MEA@d1E`#K^!m#F+>!%&G(7)&u>bgubPYYKIgcQdMI$J}!)G52 z3H55-weU|3@WY#Xbqv@J&6YUm2|X#|W2ajKleP)idQi!7q>U30;|lYEL3Tz&^r@;sMr!B#Zz@UrHrj5&?o(oReABu`*ilhNvEj22U`@r7_URjJ%t@Eh7V)ZJN|(i&KECtO@H(I$RZDw_F-SaMg+36FZKl z)_`6>ucJ{8$_UErN2N_;*99Re!O1S*00;D^N1&*g%Yt)=!)^7X#t=-$P$d(~oyg5H zC4CCdjZf1U+Ock#FiNFB3fM9>reT5q09c`w-3gN7V8PcLL^eVGZ_Y2bDnuWDBEfet$4_|f;))JMRk1fusB|aHN$;z zZ!k%hHs@uOZp?cSKfPjFDo1*43%#~H@#r8$AUyUT+v{7l*JfCX%9IE|5?;qE73OG>ILfV7Xl&+@m3E`xLG(Y!Xcel93G>!V|byG*e&-3 zog;6x5rdGwiyob8UBqCkU^$g@g*nA=YK&z8R`=SZ`Rf+5xw(?&8>nTsbI&AcJprvC z1jH(zE_U@km21MLKm&?M2*ux5J*E6o2XZRPH9;Uf;l1&zeZ8E0eha>Bb)DZk7w~N>*Xbg(~ z0RBUUjQ1UlW<;JjY#QH8UNLN^1*0J*1Nes`eMdUF!bwWW8D(gk5;UA|O1ia@83o;< z8D-O^NQ3AsJt>t~^d1ca+GNozDa@>`j2~*!LmWsVg+N|fxoSSugnVDbW+fBC?iU9I zq#;y&YRe)5vr8u$=ub=W_A+0{7*btSqtu z6}1!dH$nBJBu|LCX5p?_9VEzcr_|A0T0s8D}GO9qRjgbdDfhgJ(Os=QZ0aPcusDS69jyqHJz6S*Bt1`eji!>Q_P5!Q%fIX^#7PxhiD z^f=9P%e!ZlaGW;8o|vVFCcr*^S>GK!X*pebVL2po{59#&72&BWtbWmf+>WXZd7UAQ zEK7_JE_s3LRFz!x#6^L8fr-XT9(CRz&IeP!r_!Z$X&6VZ85#BVrzpaO2pDSL{`IWI zZi|bObOLy18_^?z2BqJi?rM0$BzlWr9sZPYySs)1$T@AfrI%$QR>tK?!N>L$JIj}i zRRim73b@EV)XTkA_Aew0tfp|i$)NE2ZV2MsktN0s8Zm%?^iY4jVYgNUwN{uY&c&H` z7@@bBq!QvO6f5n->S@mWOLk4e5xVjD)`#IQZ*M%tRo4m9fIR**JHba z^BF?9Br^ko2WrgYJgkQIjH^w>(m&MPb}^|DcMUH_3m;=Iz~-osAH(Avpx?n2P%&4HlARzFcBY`AzByt0#Dh^oV_!PPTN1%mm9K9G0!nl$dS5m&B zIfXd~{psr}s)gH@kfnNc2D4ys%JLsd0-ZWx3O(okmk1#p~@mS|_4TQZ^j4{!6SED%K)FT! zCNw1+3urdh^T;=80me^%_O2CjK|#6ZS*h4qBjQL^n*fE*0ncArMY(Iz;Zcemnnu`r z3J@W*H!iJ`s18X3W;LYfs--m*Qa2grUepmn#g$bU1ddhZg-d7VIo+R~Dwn0M4iqZH zJM%O~C{=RV*|w%Uk)of-0ZL77Mt0kmYEqTMe8i~cJt!oeFmXtlZ400sME9xV8s^X@ zD&YZRN_!dWxamh5V6ZsB=c}bG$_5TOZhZ%>EW5cnw;9_VE0eAm>@sl20)!tnLEgMD zF{`Z*0BK{f)r?aG0m7?kIdvZ(^{ImWu`ZyH*kPV^Q@bk`3#1YN$1-cHXp9T4RA-Uj ze)P^v*is?_jmt^-I<+vEk(qE$p`%E^-1Y6wgapRpPWTz?QUVpX95O-FN#A~b>5a`z034o9If|B6 zND+zoh}dI%)l_;3UL;Y1G-85zABd-Old(Ug7=^wZ2?+%A9ZqR(k{m>g08`}!bDFhd zKKRV>%^Xg_k@XGx)NnP;(J@OP$RH3-*sVLFSA%fRgk7cb><@bEOAOKm)DHTAQL*~< zu9K5Di=OaNG*jzu0;aV`#h zM{3Qh4pT22AW~J=cIYuqnG}W`_Q78DFt}$XMF>jyNG*jQmpWt1#9_GKe=jNsJ{Ad7 zGsFl@*CS;$Qpp)B61Pr3RI)UaF^ml;PIf!dhy+Z0*k&67D_G9DXLrbkSkPl~%b$9F zlJV-0D;#qh4Ay$aNFfPSI3y0e>e%JtqO_h|ZdcotWR(o+F8EFgNDq~|Hq`b|>QTXU z^&1o3qP&GlmsZ#i74w3oyKKi;;*uMdqHqsv&~VbSg-{!1Bx3`eac6z_`*aPb6yl*| z*|kJBDh6w&IHrB=)yV4E08)o;{3JLy3^ixHNez@=6~|K1q`NpAwGOn#kAu3%Okm*a z-$?YPGlt4@&rmkSCE}&`(iClJ3Mm~=Gg4>S_*=q) z25nFcqgKf%J7>4{tyEB-D8hlKPio`JsqS_ZlN-)*Nkklh=GCTYg^aFM8|36qdI=_d zlbey!y37Ivhy#vnd(yxlY{Uc2xfVa-vT?wL0eN4|$Q;oYVUh?{8)WsMbATI$T!W8q zY8eiw%Sd+z0DrYW63)8x)xBAxW#kZk>)$lmjOVLnON)xb6j1W$YmoLQ?>}@ zTu90xIop^gb6ig}J z+v|$(@Z>~Sor@2`qXMm1508>bRL7=0X9ydcRXjFEZCGY}YAfkYju@@cTrOpAzHz9I z!1k_63$nj3PqryefU7LToJM6;DJhT zpx_an=8l5vqJXj0k1sBM)G;ZNDAfjZ=XL6PRMEQSb)18?{VMq1;n1vV6&P)fWYBzx zZf*E8nNl*$;}R*{IQH*T2>3*-A!8_dj02jn^^H6?I*B>YTA8p*DF@5QgPc@JYqHUp zQCgVG`Iw!~J?J>Gv;Ea1oU!OLT8fz- zBeej#;Aa}h{OL^;yg+r3Gq6*TMKzR23W4&CiRDZR@r2>;BW<`i{{XctQAC_5$RXK@ z$x+g}UoJxp$5Z!cqsk3gI@_7g_O2FT9vcHEao)39fq}6gWnV8Z_o9*75^&1ehnFLf zpfZgIN*z3_kuJ6@cOIgGWR{{xe9Tri7-8$)q88G`LI5PkBa$$GTGI>&5fF2peKASn z84HYp_|iz_fl??)Ljfx2U@@h4$fq4MuAmr$$dX6&sbyy30Nb8p+uoZTj&P@Pc@Tf4 zEQ)5tEsT-R9+lN!vQSO|+zR5{MkZ*MR1z-ak<-&W>#FS{>4X)*!!a9=YGX!+DIUX9 zbWk_17tlUjw>`NJdI5-2O6p_xh~|E^JF(rna|G6HG8+Lhz9l0_87rTd^rwKKxNf{) zMmcT2N}4##vqp=6r*i#9DfW+|Ib4)3U&=SwP@cDpTf0UMv(Kh(yb{EIRW3mBxM{8je&9j>eU763&`(bF&p*y)aKo>huZ%kTiuHg+#}j zIzneq+)^c#kZL}s)KNJR$FDkDJA=xDh?7P#FvK1ERRu!#33Bo*n2Rb2mM@+>b@le9 zG*e3yUlj^pGOh%Wu7XEGM^9>GR&hmv7{Thto+;M(DDj47(%BM$^LkdT&l%&l)0Kh@ z3=`X>LvwH*=H4}sX-3rLH_kQ-^{S@%nzP~m0L-WV0IWZ~dESW7jIm8pWC6&If|#y2 znAnl|pL*cuONajeGf_F@WlXB^Fmr)QaAsp7=Ky4Pr$h1|=Anc4U-H(O1UCy&04?(!lynrhzj-IH{^p*3VNwdBqq7sOiRMo)Zqz_4>0!7Br2zfod({^I0Ji&sbseWO-lntv0L)+i0Q(>R0FhLmko%wQQWD9kJe+4ZIPFl&32Uds zELKe|mfKGCpr5!uO5^;?{I$3Wk)~X5@Dpr;GIph{HF~A&$dg(dXasxXGHTmdd`Ne4k^(qoqeq`sBD&7#JY6K}6J?Zx!yMJ*+zdHL0 zsEFi&Vqg{BhB7?`aaLq($*3ar<&jzve82LVO~>ye{!vs^mlo2pvm!{#x|Z0FY9T0N z!v_qbY;vT{`^5hMGV9QZybPRMsxEF><6uI{OMX>{nCFg=|Q9| zv{1w~$(MD`4^v8HIMuhC*On>Q-@YHoQjd{;oYt#6RUpD|zBzA8*3952&Xc+4if=>h zOSSpt`hRK*&O#{3$?P#o1%b)kk1hFEWafQ^b$Oc8AqUD=R!Kc}q7jfdfk%{gqHo?M zB2V8MQ_{@qu#7UEcs+e;eNB`=m|gO5k@fv4@67)I*GgFZ)SmwUr4bBUE26WpD7=9< ZCY*Q2lK^s6I0Cu<0I + + \ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/bumpy_world.urdf.xacro b/urdf/world/bumpy_world.urdf.xacro new file mode 100644 index 000000000..ed0421e30 --- /dev/null +++ b/urdf/world/bumpy_world.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/hammer.urdf.xacro b/urdf/world/hammer.urdf.xacro new file mode 100644 index 000000000..d591136e1 --- /dev/null +++ b/urdf/world/hammer.urdf.xacro @@ -0,0 +1,35 @@ + + + \ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/large_rock.urdf.xacro b/urdf/world/large_rock.urdf.xacro new file mode 100644 index 000000000..4a4cf34e0 --- /dev/null +++ b/urdf/world/large_rock.urdf.xacro @@ -0,0 +1,23 @@ + + + \ + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/medium_rock.urdf.xacro b/urdf/world/medium_rock.urdf.xacro new file mode 100644 index 000000000..4cd8551b4 --- /dev/null +++ b/urdf/world/medium_rock.urdf.xacro @@ -0,0 +1,23 @@ + + + \ + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/rock.urdf.xacro b/urdf/world/rock.urdf.xacro new file mode 100644 index 000000000..c4de4f332 --- /dev/null +++ b/urdf/world/rock.urdf.xacro @@ -0,0 +1,23 @@ + + + \ + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/small_rock.urdf.xacro b/urdf/world/small_rock.urdf.xacro new file mode 100644 index 000000000..609e15b0d --- /dev/null +++ b/urdf/world/small_rock.urdf.xacro @@ -0,0 +1,23 @@ + + + \ + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/tag_0.urdf.xacro b/urdf/world/tag_0.urdf.xacro new file mode 100644 index 000000000..b081eb58d --- /dev/null +++ b/urdf/world/tag_0.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/tag_1.urdf.xacro b/urdf/world/tag_1.urdf.xacro new file mode 100644 index 000000000..6c0afe34f --- /dev/null +++ b/urdf/world/tag_1.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/urdf/world/world.urdf.xacro b/urdf/world/world.urdf.xacro new file mode 100644 index 000000000..7bd248c56 --- /dev/null +++ b/urdf/world/world.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/urdf/zed/include/materials.urdf.xacro b/urdf/zed/include/materials.urdf.xacro new file mode 100644 index 000000000..8ccb50b18 --- /dev/null +++ b/urdf/zed/include/materials.urdf.xacro @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + diff --git a/urdf/zed/zed_descr.urdf.xacro b/urdf/zed/zed_descr.urdf.xacro new file mode 100644 index 000000000..cff4fd376 --- /dev/null +++ b/urdf/zed/zed_descr.urdf.xacro @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/zed/zed_macro.urdf.xacro b/urdf/zed/zed_macro.urdf.xacro new file mode 100644 index 000000000..3c852d45c --- /dev/null +++ b/urdf/zed/zed_macro.urdf.xacro @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 65c0f54b3370eed16c3375b4b10837c62b3f821a Mon Sep 17 00:00:00 2001 From: Vishal Date: Tue, 3 Feb 2026 19:57:32 -0500 Subject: [PATCH 05/31] fixed not important changes --- scripts/sim_creator.py | 251 ---------------------- simulator/simulator.physics.cpp | 2 +- urdf/meshes/large_rock.fbx | 3 - urdf/meshes/large_rock_decimated.fbx | 3 - urdf/meshes/medium_rock.fbx | 3 - urdf/meshes/medium_rock_decimated.fbx | 3 - urdf/meshes/small_rock.fbx | 3 - urdf/meshes/small_rock_decimated.fbx | 3 - urdf/meshes/textured_ground.fbx | 3 - urdf/meshes/textured_ground_decimated.fbx | 3 - urdf/textures/clay-rock.jpg | Bin 22869 -> 0 bytes urdf/world/bumpy_world.urdf.xacro | 19 -- urdf/world/large_rock.urdf.xacro | 23 -- urdf/world/medium_rock.urdf.xacro | 23 -- urdf/world/small_rock.urdf.xacro | 23 -- 15 files changed, 1 insertion(+), 364 deletions(-) delete mode 100644 scripts/sim_creator.py delete mode 100644 urdf/meshes/large_rock.fbx delete mode 100644 urdf/meshes/large_rock_decimated.fbx delete mode 100644 urdf/meshes/medium_rock.fbx delete mode 100644 urdf/meshes/medium_rock_decimated.fbx delete mode 100644 urdf/meshes/small_rock.fbx delete mode 100644 urdf/meshes/small_rock_decimated.fbx delete mode 100644 urdf/meshes/textured_ground.fbx delete mode 100644 urdf/meshes/textured_ground_decimated.fbx delete mode 100644 urdf/textures/clay-rock.jpg delete mode 100644 urdf/world/bumpy_world.urdf.xacro delete mode 100644 urdf/world/large_rock.urdf.xacro delete mode 100644 urdf/world/medium_rock.urdf.xacro delete mode 100644 urdf/world/small_rock.urdf.xacro diff --git a/scripts/sim_creator.py b/scripts/sim_creator.py deleted file mode 100644 index 8dbcdc9a5..000000000 --- a/scripts/sim_creator.py +++ /dev/null @@ -1,251 +0,0 @@ -import tkinter as tk - - -def create_60x60_grid(): - root = tk.Tk() - root.title("30×30 Grid with Toggleable Colors (including black)") - - # ------------------- - # CONFIGURATION - # ------------------- - GRID_SIZE = 35 # 30 cells in each dimension - CELL_SIZE = 25 # Each cell is 30×30 pixels - MARGIN = 30 # Extra space around the grid for the border - TOTAL_PIXELS = GRID_SIZE * CELL_SIZE - - # Now includes "black" to represent the rover's position - color_modes = ["white", "green", "blue", "red"] - current_mode_index = 0 # Start with mode = "white" - - # 2D array to keep track of each cell's color (initially "white") - grid_colors = [["white" for _ in range(GRID_SIZE)] for __ in range(GRID_SIZE)] - - # Include black → 4 - color_key = {"white": 0, "green": 1, "blue": 2, "red": 3} - - # ------------------- - # TK WIDGETS - # ------------------- - main_frame = tk.Frame(root) - main_frame.pack() - - # Increase the Canvas size to accommodate margins - CANVAS_SIZE = TOTAL_PIXELS + 2 * MARGIN - - # Left: Canvas for the 30×30 grid - canvas = tk.Canvas(main_frame, width=CANVAS_SIZE, height=CANVAS_SIZE, bg="white") - canvas.pack(side=tk.LEFT) - - # Right: A small canvas (icon) to show current mode color - icon_size = 50 - mode_canvas = tk.Canvas(main_frame, width=icon_size, height=icon_size, bg="white") - mode_canvas.pack(side=tk.RIGHT, padx=10) - - # Draw a rectangle showing the current mode color - mode_rect = mode_canvas.create_rectangle( - 0, 0, icon_size, icon_size, fill=color_modes[current_mode_index], outline="black" - ) - - def update_mode_indicator(): - """Update the color of the 'mode' icon.""" - mode_canvas.itemconfig(mode_rect, fill=color_modes[current_mode_index]) - - # ------------------- - # CREATE THE GRID - # ------------------- - rect_ids = {} # dict: (row, col) -> rectangle_id - - for row in range(GRID_SIZE): - for col in range(GRID_SIZE): - x1 = MARGIN + col * CELL_SIZE - y1 = MARGIN + row * CELL_SIZE - x2 = x1 + CELL_SIZE - y2 = y1 + CELL_SIZE - - rect_id = canvas.create_rectangle(x1, y1, x2, y2, fill="white", outline="black") - rect_ids[(row, col)] = rect_id - - # ------------------- - # EVENT HANDLERS - # ------------------- - def on_click(event): - """ - When the user clicks on a cell, toggle it between white - and the current mode color. - """ - x, y = event.x, event.y - # Check if the click is inside the grid - if not (MARGIN <= x < MARGIN + GRID_SIZE * CELL_SIZE and MARGIN <= y < MARGIN + GRID_SIZE * CELL_SIZE): - return - - col_clicked = (x - MARGIN) // CELL_SIZE - row_clicked = (y - MARGIN) // CELL_SIZE - - rect_id = rect_ids[(row_clicked, col_clicked)] - current_fill = grid_colors[row_clicked][col_clicked] - desired_fill = color_modes[current_mode_index] - - # Toggle logic: if it's already the current mode color, revert to white - # otherwise set it to current mode color - if current_fill == desired_fill: - new_fill = "white" - else: - new_fill = desired_fill - - # Update - canvas.itemconfig(rect_id, fill=new_fill) - grid_colors[row_clicked][col_clicked] = new_fill - - canvas.bind("", on_click) - - def cycle_mode(event): - """ - Pressing SPACE cycles to the next color mode. - """ - nonlocal current_mode_index - current_mode_index = (current_mode_index + 1) % len(color_modes) - update_mode_indicator() - - # ------------------- - # HOVER COORDINATES - # ------------------- - hover_label = tk.Label(root, text="Hovering at: (N/A, N/A)") - hover_label.pack() - - def on_hover(event): - """ - Update the label with the current grid cell coordinates where the mouse is hovering, - using (0, 0) as the center of the grid and aligning coordinates with Cartesian system. - """ - x, y = event.x, event.y - if MARGIN <= x < MARGIN + GRID_SIZE * CELL_SIZE and MARGIN <= y < MARGIN + GRID_SIZE * CELL_SIZE: - col_hovered = (x - MARGIN) // CELL_SIZE - row_hovered = (y - MARGIN) // CELL_SIZE - - # Convert to Cartesian-like coordinates - center_offset = GRID_SIZE // 2 - adjusted_x = col_hovered - center_offset # X-coordinate - adjusted_y = center_offset - row_hovered # Y-coordinate - - hover_label.config(text=f"Hovering at: ({adjusted_x}, {adjusted_y})") - else: - hover_label.config(text="Hovering at: (N/A, N/A)") - - canvas.bind("", on_hover) - - def on_q_press(event): - """ - Pressing 'q' prints the 2D array (using color_key), then creates new_sim.yaml, - and closes the window. - """ - # 1) Print the 2D array - numeric_grid = [] - for r in range(GRID_SIZE): - numeric_row = [color_key[color] for color in grid_colors[r]] - numeric_grid.append(numeric_row) - print(numeric_row) - - # 2) Prepare YAML header - yaml_header = """# All units are in SI -# =================== -# Time: second, hz -# Angle: radian -# Distance: meter - -simulator: - ros__parameters: - save_rate: 1.0 - save_history: 4096 - headless: false - - ref_heading: 90.0 # For the GPS sensor to work - - objects: - rover: - type: urdf - uri: package://mrover/urdf/rover/rover.urdf.xacro - position: [ 0.0, 0.0, 0.1 ] - world: - type: urdf - uri: package://mrover/urdf/world/world.urdf.xacro - bottle: - type: urdf - uri: package://mrover/urdf/world/bottle.urdf.xacro - position: [9.0, 10.0, 0.5] - -""" - - # 3) Generate objects for rocks, ignoring cells with value 0 (white or no rock). - rock_counter = 1 - yaml_rocks = [] - - # URIs by value: 1 => small, 2 => medium, 3 => large - uri_map = { - 1: "package://mrover/urdf/world/small_rock.urdf.xacro", - 2: "package://mrover/urdf/world/medium_rock.urdf.xacro", - 3: "package://mrover/urdf/world/large_rock.urdf.xacro", - } - # For demonstration, different z's by size - z_map = {1: "0.5", 2: "1.0", 3: "1.0"} - - center_offset = GRID_SIZE // 2 # Center offset for coordinate transformation - - for row_i in range(GRID_SIZE): - for col_i in range(GRID_SIZE): - val = numeric_grid[row_i][col_i] - # Only create a rock if val in {1,2,3} - if val in uri_map: - # Adjust coordinates for the YAML output - x = col_i - center_offset # X-coordinate - y = center_offset - row_i # Y-coordinate (invert Y-axis) - rock_name = f"rock_{rock_counter}" - rock_counter += 1 - - lines = [ - f" {rock_name}:", - f" type: urdf", - f" uri: {uri_map[val]}", - f" position: [ {x:.2f}, {y:.2f}, {z_map[val]} ]\n", - ] - yaml_rocks.append("\n".join(lines)) - - # 4) Add finishing lines - yaml_footer = """ ref_lat: 38.4225202 - ref_lon: -110.7844653 - ref_alt: 0.0 - world_frame: "map" - rover_frame: "sim_base_link" -""" - - # 5) Write out to new_sim.yaml - with open("config/simulator.yaml", "w") as f: - f.write(yaml_header) - if yaml_rocks: - f.write(" # Auto-generated rocks from the grid\n") - f.write("\n".join(yaml_rocks)) - f.write("\n") - f.write(yaml_footer) - - print("YAML file successfully written to config/simulator.yaml.") - # Close the window - root.destroy() - - # Bind events - root.bind("", cycle_mode) - root.bind("q", on_q_press) - - # ------------------- - # LABEL IN CENTER - # ------------------- - center = GRID_SIZE // 2 - center_x = MARGIN + center * CELL_SIZE + (CELL_SIZE // 2) - center_y = MARGIN + center * CELL_SIZE + (CELL_SIZE // 2) - canvas.create_text(center_x, center_y, text="(0,0)", fill="black", anchor="center") - - # Show the initial mode color in the icon - update_mode_indicator() - root.mainloop() - - -if __name__ == "__main__": - create_60x60_grid() diff --git a/simulator/simulator.physics.cpp b/simulator/simulator.physics.cpp index 52363fc9e..1696af7d7 100644 --- a/simulator/simulator.physics.cpp +++ b/simulator/simulator.physics.cpp @@ -13,7 +13,7 @@ namespace mrover { // Important formula that needs to hold true to avoid dropping: timeStep < maxSubSteps * fixedTimeStep constexpr int MAX_SUB_STEPS = 1024; - constexpr double TAU = 0.5 * std::numbers::pi; + constexpr double TAU = 2 * std::numbers::pi; auto btTransformToSe3(btTransform const& transform) -> SE3d { btVector3 const& p = transform.getOrigin(); diff --git a/urdf/meshes/large_rock.fbx b/urdf/meshes/large_rock.fbx deleted file mode 100644 index a4309c580..000000000 --- a/urdf/meshes/large_rock.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6d863fb8c715ce599cb6622d1951267ce0b912b1a4a3afd321a92df63653bd2b -size 336316 diff --git a/urdf/meshes/large_rock_decimated.fbx b/urdf/meshes/large_rock_decimated.fbx deleted file mode 100644 index b85bf27ad..000000000 --- a/urdf/meshes/large_rock_decimated.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:181c30a95da1588e80c778923c9708b74cc610a08489a73d5d5d622f6fa49d8f -size 67420 diff --git a/urdf/meshes/medium_rock.fbx b/urdf/meshes/medium_rock.fbx deleted file mode 100644 index 7ad1c3316..000000000 --- a/urdf/meshes/medium_rock.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4511a80fad809d60ecbd4a6336f83f3adf2e7276b051fb86224355c1043e0963 -size 317004 diff --git a/urdf/meshes/medium_rock_decimated.fbx b/urdf/meshes/medium_rock_decimated.fbx deleted file mode 100644 index 8cb15d59e..000000000 --- a/urdf/meshes/medium_rock_decimated.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5415a186af45ffe67c4fd5980711f0c7e3ea91f8c8032b726715380053f906ca -size 76380 diff --git a/urdf/meshes/small_rock.fbx b/urdf/meshes/small_rock.fbx deleted file mode 100644 index e141fd843..000000000 --- a/urdf/meshes/small_rock.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:22a7c07a5211765c501f6623ec9c7abf51705e392acf3848f89cc059526b1f63 -size 318620 diff --git a/urdf/meshes/small_rock_decimated.fbx b/urdf/meshes/small_rock_decimated.fbx deleted file mode 100644 index c8c550bcc..000000000 --- a/urdf/meshes/small_rock_decimated.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6fa0fc4a18f080c9abb31479f48da9174ad8f6c6f4315497413bbe686c0dc206 -size 76796 diff --git a/urdf/meshes/textured_ground.fbx b/urdf/meshes/textured_ground.fbx deleted file mode 100644 index 17ed3b134..000000000 --- a/urdf/meshes/textured_ground.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6b2b267c0f5b26db4ba221031de72cc76e2dd7752bb978c685fd871371ee96c1 -size 13159980 diff --git a/urdf/meshes/textured_ground_decimated.fbx b/urdf/meshes/textured_ground_decimated.fbx deleted file mode 100644 index 0b295e00b..000000000 --- a/urdf/meshes/textured_ground_decimated.fbx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:935cce106bc7e4e258a4f07b3c7789f050a8dc8f537527911141328035730db8 -size 269324 diff --git a/urdf/textures/clay-rock.jpg b/urdf/textures/clay-rock.jpg deleted file mode 100644 index 54c5dc8df530c8311fc6fbd31189494e0e50e65c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 22869 zcmb4qRa6|l6Yk>f4vR1D?k>ByJH-}?yHjX^;_L#8OOfL4Zl$<8lmey2-K9wB@Am(2 z&$+L6@|NU$nUj-bCiBhwxAxyJ0HM07nkoPZ2?_AO2Ka9Spaj6iz`(>n$Hv6O#KFeK z!KWm|$HT*?B_k)IWTs~UG0`(IvT}*>v2qA=Ff#Eg2nb6^$;!&I@G7e*NkhbBWTpNG zf`pBOgO7&~BqRh%u`{tt{eR1U0{|jSBrK#P6eK19G7%CA5z>Fd0BQgL<)5qq&2H9#Ul zW)vV+(6jYMVft?!fQ#~PG7$<9Ko0OMWvB6kydT`LMSsu=Y%wnnQn^jl(-u2SHlJz6 zy1Kdq<&M_$*=-3*fJ0tCur;Xco zj2)Y!QBET`f()GpPznf&_BD+ra3@m5EEhQA240M9BtA~R%cZI(MTyq$g9uMW&hTu) zwaI)TW;F3JYjo*}r$es7K23V4u}VZ=u)67?N@U~9BPM!>NgvFY_xVGef9p3l8s3~7 zIF$(s?k4Y#Fo>Rd#8w+8>v^2 zA$Khk9$1m%kj-#GfR1+p652tzhcHs!9ts^Gfg}4tB#~yP<()#y`Q5e|J;3TO&2(JQ;^I9I8Ad-fl9BV9%}C4wY{OE#nWak zD6BFG@uAIHDk{wkr1rN-^%`qa^W{SlVQO(Y2>xr7e@(~b&7R#G(5ULqsjk`;Hm7i| zKQ>>aiTg>p&^x#zv~aPcKzx16B&r#6tx>5fdz~p!ec*4$i_HS$r` z>J6OAgMpW3yd;lzW!zE4sVL7ZqnlLx+2CS%jzn>QiC=Q_!n;7;kG0P9X(Jg`Xy^BE zFXqwLnmsfali?WuybI;YG7(=_;Y^O{8^GTJVl>4o1}|B6xBa;N~!=p1CdqXjkp^w2*4B5E~rXrvb$7v%1fg1H_0BX%0YDx(Ane9(=vQQ@FK z+%iZF^rD2MU(!~( zCN&E|<_g1TJJsA{eHGCO3T_D%O4!jt=C*p|#>Kk?Vh3Z#afGSg(v(a2A`?P-Q7s>A ztFe9c(f;FsbakB2z}q)bj(YzA8q70%O;?p@o5Ce?aq_~U)Y>uFf)bws>c__+_F6be zDzOh$`wSD6aPzBb#hWqpNkI;fK=Vh~t-D2py4|)2hqDExuH*s-2TioJRaE2HJT>MX zY@!*<>^XDZPF2Hio$e7xAj@vd;DCD613Hz zDF=ZG}yAEuxC0MNf7d*^lG?uYLt7(i zmo7TYT7(;I6KOWN1<)@d{5={_j+9wqCQ$?sm!5`M*?o?u zf2c7`kI9ji{hokDC!WS4Ygcn5h3#K=CuY0@d1Ed#(RB$H$D-d*IKah+V(5pg^2M3` zP!O^Tl7@I;5TiY@{Ji)}alfx=EZpGP>Od7)5VVv#l5jb?pRPd>n^)sIX*l`monQ6m z-q!1PtOe+AVZjdi{MU^Km`Cpt?!=14i(A_R3v6!&wfQAUj^D6p2euK$^Re)64%$g# zIoL+Zyt>MI;%9e7QW?w8CvzU(y$mfIm!F~s#h5F?F+W z`g1vt|EZSOq0otWvg6*&=vcWdFw9GwZ*TCHg|mz5MNiR2Ci}Me>%};VoN$6oH4j&^UIx#oP85zs{U2NWer9w%&I3Nm>>Pe3c?A3)sn(yo1z{Vu7 z++W3Mp(Ct;zjqh-@bqATeS`d0PVKg`mKJhB+YbEFj|JaJu{?ng;}wYJpXe!q&|Hy@ zbXmtv)2~#7qdksYtT7TLx)gD0=i~4F*;*&^2_mR|f)0;3UD4m=y)96PwVMimGWfWj z&obC3piNn^m2^K06u+{B z?b-N(e%EV`*siJ?`ZMP%3w#)AROT!_6I#t1W;BTrK9uVJ4`7ZE7m2LA+HnSL9j!F-aiRG@CYu@~xhu35gCU0ZP^W41Za$SRcaP zCIrmX%cF~Q?1*S!9=rh`ynOCiO?_(+e$xMz667O1-1}y^N-h1^i?r}`W3{`CsI|2z zhQ=8<6QZ&To;B)X4|QMv?0t|c%J8x4NUV5h?5$}5;{-$H2(}WcdNJlAXfxe#U_$P` zkBA#aAw^V4G+By8I8DxdG{?%d+Itv5uEARroCYhnTg%Zv*`EO3E@nw$3ZIpBu+X`X zjQ%>}N?-cabX-`+lT(^P!psg#M@qnAOmG?_XJpHBND1w%#&wyGC>4;H4aUYcBX?If zwuz54^Rq8(P`{~+76Fr2ct_5pj-47h4d8<{Lk|1Um8uA+aODX9wy`mFQt-zU^_I9H zbt9(VEvQfL`=w$YB62C0t1y_EFY$ZM2+jixh)xaD!u+*(-NqEd{ul7 z6i7xd5Fg7_t6(IdUB*o_>2G@*W~)H$SGj4Tf6cly6h8%rMG;b*u2%i34bS~leKP2a z$inW=yRR6tv)5zkHBdwJ!X;qzbNMdUhVHP8gP|PZOmwB;PD(a}xZJs!`L{@if~$>F z117?ekje%tvMywSF9dF@w5{!{6k>EN-m;Z;H^r5+?FZVMR~!vH`diUX`VbA^wKtr( zKs#hLXXILmyqO-g{z75&?z^X5hgYLY<5Q_^D#l~&OQ=muPCbEGN{XONH1X(XWF=}3 zA7>D$!IwLBI(db@ z5VQiTB z-4kVg&O0O9!3^TCg{eO1ddxMuMy;0nJ9OOkRG{GV$KUC9B1gQ|!z?S3*Nm!T*yIzT zfzolb^2PrFwBO4nun@=ic`{OBl!Z8l1BIVVvZ^XAM(nWm?QKeI@R4YnApe{{N=u}! z0TA}=r)uf|m@7^u8)T`E`|Gg#e!$AqOacUK`oN6>ObO0qKS0nz4QIjA=PGE$4C zI0W3CZ~8EYt{`JMg^QHeul$isY7x5`^7WI?L2q(f&?pKZv}{%|cz3^;@C|`<<%ifOr@i#RC*_<4xesoaX{|6u&jwlXiOrJjFUc`%?*PCMR zNaOU*UxxOf;6R|6Hzz6y;xXoL#Q}k1$7W9YXaf8JMhyGecfTunx1=^4+SOPXXDJj$ zizF`R^}uo?79+yyFx^zkqie|R_gco zlZrmPr{jfxSA(W-K-!3l-m{`)hP=%garR4MT>hwyb<@x%(F$VF9w`1KVEgHyl@38H zzLzfH-f#Pt!}|w!iMF2*(t$MGxHdX8LXmxK*F~+Rqw_^ei(Zp`W2-qns4M`7oEaX- z13QRUP@caasJ4G^*UiwCAm21i&pZWhzbnC!kVpZ&d0B0Pc7OWhApbJf2TT|h$mB}f z$o}0xOZ17HjX6tN4-8Dr6kqO~tE?g|wRec40->$ZhY(Xvj^soh9?~R~#13hS2~E+m z+b1ZZ$0}c_>wYX{U)~_;{E2G9oJWZf036f&mby;5!98%HBwFptgo|wAkzGPa6;w|) zfKrK)&#|+um7;-fQJB^Ksvs&d(soDTiOu+1b-$S{gq1rGeZbCLRt+On^7mb*(rp&j+-cwKG!B=qH zY`MU_pEI1_3^btEC{yV{q?3_Ny3|C=PROoc>4cNq8)EX&g7cXB7Mh2&nue6oGNwd) zh*cBzVdNal2*Jn>$67BwOcbeWh_@J{?Ln$Qm*+kfxw&<}6^2jJY)e+!iEWNQ7$m#l_ z{g&u}c2riM%Y5AO2ilA24)`l4y}>)<4&>EUw%iB1$r6JUeuAQtuo|q2-cGlMShnbO ziA5j;bscLh#PQi6?^Nn>E%gDj32*b5klD6|x({E%u=(*aW`iqK>rR`{X2@+wGb{r7 ztY`Vi7oF$Ofx`(0ksD9ZSJftd`0j}pz4E!kSlc3eJ&Zr#c}eK}LxSA$ADmHp0kC*i zkgC})I}qdP59~qsYzr;A%b?Tm;JJ)P)L1`uOs~u zEr$bwl;S;yfLV$Xxj(-fFk(r%>Da=@o>U8|_Y1>V)&`kgG*icXDFMfFNb0)~!}WY6 zG88Z2JLD96(uE`@X~<^(5{No@_YbeFvh%l`lx=W%@Zo9{JR~E|Fa6s*k{lzv@wb!f_bH0-mN~jsJTG)ayCKy0&lhv$v=M(o`D&X`|Gaz;V8J(zJ;lQl6+GXm^T(Xm788a=}|fCSDr^~E|o+m=PqCltclf; zUTF=zg+(GwjK8|#d4SsC8EYWsfAOAsnU@tC0Oo)L11{OPFt%U0qBDaB0r&3)EJSdZ z3=#^WHU%PFg+=4pVdE5G_cbpCfa@cyoTcm6?t8OGbg9Ley>av|kc=szfG@3mCwde{ z3(AnLjvz!8U;yqyPQgt38j?g2)4nvzZAhxAMn>*knKGP@UzM4}mMWn#?=waKyRv$7 zI-&X(ytsPXNO#C>l&wNqc%_;S*|pAHtG>C4Ei;j|)uIf4h7%IE8!jE0jN{YPyo+oK z)w@}!VR8hl+4E$9l=@+S;+%QE&3dzX+lBndQNx{1Z5r+;nf0o81i?DP&lF~f&z)eW zYnRW7{F8g`XHG*mEcHwRRk&+3K>Su#UHP{Rzwu_YMEyIOVSRhC?S~R~167}|3-JQ% zqfI-ufI>@srtJiJ9u@@VE~gR(BuKyGX>|L(cB`5$io}1@ZcWojFiQBL3QOC}i<`I^ zpDOF`I~l1aDof^76r#&NO%t#qqcY@Q78IrZ+v-Tt?8by_McAn;H) zC9!HqntBw951q;nAnax|I5#9KlW{}^w{sN#psOU-3-mp`(g$n*=mY0?Y#j3rKNQ~1 zu)9@In~Pn3Qg`C>VMJ4nv4u)yw4?^P%rU53ja(I5dtKY`KLG!w`Bg}>*~IE%0H|K@Yc=y1tp62}0oWbti4h#^UGH=dGLV9TqE0 znsXW{*Q5Tn`c(AmmnTP)=BxOEEtB~y5^)vg>)qT2s7~fYMZyRCR6V9HV}V9c(q2IQ zoKwF52r&!eS;kW<+&avBr?+sal=FL~$$zQ!5uK+))6=YIH(D(2&z;3;g%1 z(|fiL99k#`UuQW~7wQmxuXRHgd$?g3n%F0d){Q=qiqd=7SiY}~H=s`fx81(h*5-PW zWYk6kIJEJz_eI2{9@KCtx8da&(r`^i_?}i!!P=)^cMfNqs=cDOWI`BV1=z|=<(W#9w8Wc9)%(+9(!~P42sUs{{sYhxJHt8 z>vp(u$n%H80PlNe{QvS8W&U8~or-p*pQ4fp^IlKFa-a)7AOS*6wZtFDx~SUro0Axd z6=~zYV~%BQ!FhHg*-Sqt{Lp@eTH@ZX7v-#L>6fIC2Fq{eR;vvS_v>o|?(588v*yKH zn^Nzu^LpqC(nE}?uBt7h6Fn2O!UMjg8gi`p$+juiWiecY5GiyY5Ryc`%e_$IP5H3Z z-JOnZJ7=qwWx6bJK9^FX%7WdJI-(RUJ@J)H^FYKXNo)veBWIq2Q_2n5j0h)?ywFz_ zR2(S-eAplrBjb!h;;*0))68`oRxBDeQN_r{Un%hLEC~p5FE#e_I+!MNUs>~5~N!GJ7oVQ5a^h9LK^p!v2o7;RL=#i6Q9PrsI}qY zG8iw0$3z(MJxEil(Pft8sTV4WC8pF~z2k>!V7~C-&-Y zP9&I}F-gYF50-gehO$6Y(OA?7uvX>{tX7KBu5B^)wHHRzY++nEbs5*CG?Y2mhnZ0yQ%L>%wyQu7U)UAQy7Ff zXAHeToVoJ-2PpgEv}H|@Hr%VvVcV9jIbVO)@TpK$;>5Lfn`M1$@&LRV@NR*<2hxo!NL?)eyOD zFva5Z{_|2FyGOA44G|uEzJ0%{8$Ln%v!q4zH@=ZUVWSc)|icL zfZvOejG=8YU;PIl=<4dsLl@wFOSbfQPaFAsD~z}Em)X~sDUqTb(%9AMDI|9l(L~p} zA#q{jfrGWU={eY~h)pu#?~hSfCVDztdyXU~*(NclT1%DO2pp@Uksew%x;A66)FB?V zd-?=CHw0Zvv-)M3lavZD8p*8PRI<8qJt=8;lHx)*(*y>d@%!(#fnq+~z9YiLe7rf! znEzn@wnZ+$;%V-n4UdjeP!Ch8Eg{JK5IK z&RjP1fJLMUL#uSZ9#X^*z{&kv%<0W9`{UpB$`6?v(QkY{iAJRE1wQM8uy4@eIka;K z9e#gJ|zHli#vf^6DRq^71HaDJ{O9q7x(}H3-;clAp?zTQ-NOb$OVVGI^igKGbMS=Yrry3X2jS+XW^+RYw{N z^6Qqkcl`&*)oec^(la(f-Ghk!ESD-&Khtc&Ahz@_gT{<+LQ^UWmZ}L&$rMGIZT}h3 z3C8b8s{yw*?B!(Q%fEoYMrH>0H)+2Wn%wuP2L1_-!s(r1NDbOCgK>9r=ug&GG>L=bLRU}%J#E4;W3jKn$N%Q>o8iI zQmk)E-BixqSN1ABqz#^%4reu92Q1bmX`rkj^#L8-eayU;DyKvj zfiDK=*>*!DS#)m3$|hR^JDJbFKK{~l#3SL%p$wdO@?Eb7Uvrcascq{GmQcOzoXe^j zK;o6w2f z0{U%H?_oA2fIcy;t)Kt%GV$N_N5dIviza*2-dU(!)f%rvhew9re}I;L9G;T4!|07n zWyu4D(r^sfL4>P2Pg(BznuQ>5N=k+`^9Lf2*V7MAv#(U{$ER{s1-Hrv!|NL)*;{XU zqzflM+TNGHd2~Mhn~FpM`@RZ#w7$EKd+gh9yK1L`aoe%)oS4=JN}M?J zy_k^OqS(+d@ZC;NRu~ZjGo3Ab}awy3|XRD-L2|rLg zhi(Nto@$>{&EGFwoks=b1Jy!6Rwk&JXx4aj3tk;O{Xi zWkoye)1H~IqpAY(zlrqKOprsj=#eXL@qr)zQ&QV7qGNYt0Wojv?~g6 zZ@(X}t6vx38H*pCn^tW)f`=g;IExWk{yYx8codD>US%1}zT*zLT+Hxx*hz!%Fy<4B z{+Jx=CK#$bf8~7BtK7_=j)-PculkiPEwak#+^;JxJbc~h1-HMCAK;g$cI z8+*VU%eL)lXa0c;^S^;LiqG{@jXsp#@{;;*%vPSNC~|Eax}QKoZP0E>@RvT)4sl*inj_*kb0!d2)d-4x0{Ab29PGkk>gZ0kc-IaUUH6@y{OyRaB`CK09Si3aKvJA8L0uU3^yAJ;WLcS$_o3(yF-`Pz^ zV}r1Q%pe{%7%p^_j#d5xEN$`Zc+~N1eY)7RFdf(K)GiA7wg`rj20y;@i}!`w;7Z(Y zfEM9D6H|?GG$06k+_b6M!hNGxSMw=RxwRUnG^3T_-%;oBpuyyEyJ`zA0z9}e0d@>y zKBpmUJmlBy^V0gF(+@sr&*ASkV0?-fVf_$P;0#`~Irl*jMKV{@tyJc~nb6d+N}c@J z+sbAg&gldTRvaT;kZdP?ogUgstG;cqYiKfvt>}Kt+)j%o28SQFM@ktV+!5mV2N!`v zh1xh||J3r=Ng?S}ceQeYUibP}E>#7-0?=oNohl8lsw!X)4Xv7{L#PxS zh0Yvsv*#}^!wFW|uFb_|Ct+r@$ovz{K`#CyT^0X7Krfq57#H0KlCEnrIs%;Q*vFtG zmTuFpia(=;dI~4Nbq2_f@N}n7P7Py4m6pRAD& z|9lDMOd0jL&vqyQ57lg`Toxt;cFfB9-{;g52i=Np> zleXA?6%i3+3wg*%ezxJv0UJRNus6<64I~r&ogF+mgR^bgF{~_K(SZ83-lUkv!!%sJ z+FfJ5mZzlwq<$=ApQh0dbq5chT)28H z`)zIw91PuQ?`PR*>Gz1}NJW!#z+^a@IPnoEG=Ik0&Ay}tRcap{?#pIZPYa&t5axaT z3z*r_+hH9_o%JJ@`_f_lWyt-!0M!30ow(9L5tA_t95NI8U^l2kv^%vO*Bv@l7Z2CD zIRq?88{arxM3!VEYwzKbnCE!?qu}2x21rY=C=OBDZZwt&J7lBhnjxP-xRTvPqGD80 z)emErQ^EHY=8wg-sUQP2??%(-6WBd2bNnRw_2H%s?~ z&s7~OIeIP0r#7kD&9pjl_WZKBub;8+g|ep_&CbEU-3SZst?E+g+Y*vgXQ-J-Fc)CH zI;RZuGvjCZq?IF~X%qF$_^6G=j*aCybhPQ@YR)zOv7+!h2^5-m)ymliYIP9V+KE}@Z=I8 z=*%#nqj5e1-R5nwxnY(jjHwUKgm_r4$MulYsS}^AaxHTAC!W?mQ!D*r3a31T=xV>d z7;Db?zTH_-vc%1RecY#?4l&r{0U;P@M8EGB9CU;p=UlpDu1R~h!ePl7Nu&w23TS&O zWTfSSlt5VWxxrXZbriKcpv9m*$0k{;Khl49>amY2FT}DXN*6&(DWi=K_Sh-!z9xtZ zFIHjF6r)tgko^HSAsBUfF`XCX7o&M;kQ8fGsDgFxNpBKl%^REBeyE=WX#|VksQ2zB zu)2RC5+5Mbiwrgt_ED@2WJzl5x38@ne?ai<29E*n@S-jLF?rN@-UH0m8M=fRhto?1{-zNVT;mA+YwB}TRjPSh z@?&kDcb31?PJ?irz-db88<{d3H>y`7#W;S@sdIYyzd0hDaeH7mFp|^-d*fHYe*h7h zAf62YSJwpw_q*c(&pOPxDA-TwWDlfGPJlMW&m5jt&J`S4YA)6o6-s@3W7IH?0MS$h z;HZ`sciqS6$zMJbvjRpv8l)j*oCYW}glsAhh5ha7tKB8a0nWf@!=xdzHCTs0zw@Bo zm_Q>}ssTnmTYL&_O1NO)PZ16$J`Ys>-uh%FTq0Z>3g0EFLo>l3wxst9Q-!oWq5%>E zO|79V$-D1M7CTb#ktw1-b#luCGN?Azt5rD&hi&svbZLmpKQ-#EJ-WxnDl@jXC$T70 zsKe0y8O*a?b8o1AwiKZ}Q|=O@VneE$l=D`FV5D%WffVeP)Z-z>_AwjZF3kBy#InqZ z9%*+}vcR`#4!s0F6KbZkz=01vs^Y5$Uu+O~v_xzu~l{-ZH zeXK+@9`>=)?AsAt(*&|1H~0FE_3{lt5iHj<4w-sI3~3g)w^BaE8dabwx#XZIU0Jl$ z_MqTf&7m(v=hj-noD0$8hHsJrXaX8iu8y{DP=W>3h-Fs?+B4@R_)mn4>4{viaDv8D zDHme#1F|(K={hMnxyT<*ua6s#hqFNn!!ClI44JYeZ5Yq`T|2$7_3lU|$Hk_GFD}y5 z`pj(>4TSl!#*K<+U4MD4x0;t8S{@1(&o(Uo&bc_sIS!+#O4`Hwjz96-=Ogaf2Ai>x zwzG<>!!iD)W@JZV?%zhlfP6&~U3c}B{#ka`JzA~WZHQbjfk#H!SgwU(SG!SwzJG0! z{BO!bvuRHgFKQ9)nYyNfF?Hk8eL^%E33dhC59YaSkF5m$-Xj?Av00-rmT%=2la!+x zz9W&4{0C?k-aXb9XwYybUA@G;=0@D(P38Mvlez-!i>pQ{K-ecFwWH|88v!#onhP35 z%uuTP8B`w$SA}XvVr0yPX-~7INB6;|1sdNKD!i24<-iJ@zF;$EPB~&2woUIZ)g{ql zCd+&ou&rJpZ20Q5W{gPKt;o%=Ftc^{>Yb~R(OY65tquLTS=0N4pt-mLkSq~<+h)As zCBt!)EEMd)>zJbRj(Lb~p8+RJwpP&Z<-Ollg-ghGhBPr{?c;(HT_IQhz@e1m=ey4z zNUkxJa0qZZugTmKsgzwTmZXmF>ll5`20e)MFOI;071>1y;^nqhFO=F`?{S?HE6V zv)#$p-w18SWaGvy+Zy_A-DMF*_P+v8(aE*Da}gtM%(kEAJ#?!j|g24~5-P z#U`Vx*Bz~E+qG?5s%pVX8BKp&-x^cuQhkI3Hjf!TMm$Vo$ zlB-D(a4&;lyV#nmJ4*Ooe?7U5JW%@{GxZDuZWDLPhWjl2C0}lPx=2L`=?ly8X~CTa zwh9|f7%E8x&D-$VX&k~|BrxZV+uHQmrLw`w?@)cR{0O|u%_TwZg|~J_;7nDco0nY6 zmXAWh-5G{L#A29$1Lyd+w-F3pWHEk`Uf`$dO#egpqmQL{bo3vXXv~Hn@elOPO81T7 z3L`P~d0DTirX=p0H%i%ciPkt8II6@Dvh37#Lx!=&zjt3b5CViZhf@X0A-}5x4_Z45 zA+{u%v6z!HO_vCGA7_oL##9ip{6M=vESk|5^APnbTBP*JfpEa6&T9J7AO1l~la4n6JjKNO&&(~kJr;cYO(g(HDo+)};oBRWykfk2DQDVdH_HAN1b zQ!p;W(7@bMIMDi+T!Li43|Gj|-tvaM!#;|e9Uyo5d?4Jk`aYkd5C@xseBDbBG9fXh zXQcbRV>${@f&xu_-&ZL;J{A?-<_2*yAsUKIX0W3C`K6Cv+MS4Kdjs`jbq-Z)MXK(g z@^YtCr0;#bcYb^@z53xE^M}xHMuWwmC1iXYzZp_k(*=b9(Y*ai`WdPZ*LrO=2>52f z6slWI8gF+~>9B_+zONqbugtVqld7Ic&vAaPz(%19BULq?kRk+Y&LTrb zwafIm?m7Mk7}8Lx;{FqG^IaeVK3e-h&)R~!K64|ocCL$7Rjv;NN&_Gboa`ANa!FGp zJwMWX`6K+rHttWD;~B{K%@-lVN4dXL8|FH|zifUUW)O!MLd8O#59%*iOxxtsi3jd$ z7mzbFVcWb(9}L~#r4OfbU*!%*!#F#lxNHb%vhsY-&;ISQN4-TV+OG8VECI_*AGvVrz&i1w!64c^;A4TO&330$+^CJ^Hv$}tO{*dUfK6FihqQ2w%muMTx#28bM7Z$-9; z`u;83&!W4U)ldT>i=exYm5wM(oxgdXRr-AX}|FOj;l9Y|nRO4s?)MvTE(jypGU>V9tJ?+Iasq-*a0I1J@apU?qn@ z1TtSWo?aReW-b)q%1e2c=3kpX(>s_CWu^26d+ph`Ki*Z2Tt2(cxAg?mp%kEqW#b=( z^(-Cqn&u#_o1xdqm{(-1+7?x$*s#C7b-6rF_-gd`v)hH?Aw;S^U;f>qwDw<2250w# zwr>}^Q78{lH>mikjaHWeZU^p;YDr8e&^$tKSfG^gesbhys69p2UuK?59H-9! zkawyWo6fYW503iPPn2u648A`~!z_=#@S|e26YZrJbW~*^4>KMLlu?9DgzFt&mvtK2 zBtW-Uc3{f2Pc}$Lu63v$1Zd+7f1W_eAv5c&Oa*J5TyT7R!*D!Yvov|C)v>Ii zQxoZw@j)_#2**$0W?)%%TV(BXh#MzklC4~Z3BVsCpFEQi6{UYKKib#)Gd2uBQ)rH- zL7l4hmF6&y{#rpnQsnieDEkl4r5utDjSJ!HFtEU0Y(cIjgn}YqAxLHcM5PlvYiYP= z;EZN;NxRk^lWmv4xh!U!Mlv=>KkV8^2ewNOp;I+6qEYSgr4w{H&Rr^O2)1X6>PAC3 zyR4W7kgNak2G2T!;mN`(lQ7ePW*6o5vx?B{*|;9_*rybqcSx(*%1M(SApue-{TyZ* z2H33BGRgwBGd=SOt{SPkB%dQXQUq!3eK%-~+@g3}8A5#4)9nhJjpDsZynJDW*6JKN z=klQ#I=!VHGAhO#TK0xILiWxl%Vn8dP9<9QE#W|DW?&`jRS!+3vlmi&@Xbj31Dj_E zYrGyRz!^+s*=1J>MaZp}*1Nh=+EWOs_A2b-Rc1;Y;c;4Y4wBFTta`)$`P6fL1!{F= zifftVQ+&|lBWLVU(@Y6;@glrf^J7qbz}jlZ#BC=v1o*tdxyA`*rQ9E1ba#LB0&x#V zucgiARH?^(^QmBoW#FjHp3f_X%?%s=(Dn+v{7HI6_+*a?+||F`pQ8<{Vn??0#uuR* zDkIHF<2HfNeAwz*-I4n4!Cfj8Pq#xV;Rtgu3pt--uB>tQaBtXsOyr8Yps|TzMI{KK zau8f^PmM@9F=hq?Rw=tzc@)DiR>~ne0Q5WAwwPQ4r6DwP4`#mXNc64r7O%QFH#g*@ zHgQB-drKA8D%KnBlBNa@oXT)7wfZiBg1ah4M{caIN3IE`ZmOBqlgYDdl;RjJp~h$9 z3e?#JjzzWGyfRu$8CuF84YPNXaeuiU2{jgro}#{gvr(@tq$|ASormp5`>|^Og7a%N zYeu7bmG7K~(OGs23^{NHq6zcDuk}3(J6;$PehFI<=UJ>9N|qvDMxhrIy)@9E7*?K& zY@pA16I_Egb~ygB8V9$7i5a^y{;TL>ZxZxMCu2AS`O3UjUbJzCCxG*bO5!sT_J?q7 z?$>*&m?h|(KeXxZXFD;r(SKQ@O?F9D%&aE~O#l5-(x-1ODmf_fqPB?~jD6<}jFnaa zz~dxlnAE5h^34zgvQZ|j0?BBTk>7956SfrE3JPd;fE>g$(Wz!J9xN$9^|6gq{KVBZ zxdra>cB7sSHEVgGZZ{V9>gR20-< zcxUYQTk#j=hDi}y2DMtyLOsuh@uzl$SxXpQ#a(5n(nI3V*)3jP+x;mmdz|mV#aPy2 z&DBf5bpv%yC$HPtEv}D^CnFv7?yj!m3df1Z&O$1P5dj3EQF(teF)%-|`u0t&mrjnj z`ReF%0U!u!*;1SPv1_vm_$p-#)1a9=tDW?_sMW^2Cq1=gj6>`@pvg-|+}C&RN^x&# z;<^}2xlF$rKK?Lg)%5~=k6Ag$Q|30p@u3`XOiaT<$}`*Zdr~0NumW9&lYRUPyr~mf zSlUF}mW|BIzXL$(HMa+wJMyS7`mhR>ZIx0ee;v#5s040*E17}rC`yl@-%C_o7IznnltCU-5z$d%S-#12vN$?)DumN+4x3@kjK3gu`gO}Z{d<$85zMy|9)2Oj3quuNEGx#U8&=oc$x zW6cizOy2N}HyNZmZLEp!*Y!G89EeZsLT5vk7SymxkL3~<`~Clzh(5VNr8uNd&BumI z3ZojR;uFs5j!$*QFs7~zS}5(aXi%agz})qK4VXng}YtgbGQLj803*?RS%l= z>ppoy)YXSf2dL2xJ3_4EGoo$YQ*Do>E+XYmY}Hc7TXt;*dx^0|m+LYmyq%aBcwRM- zcnoZ+J^IzXs~O+lR#kx0uHWm#5(j`vEGnEIzxmPAsSF;-cx2upRw+R=xYDklI&@gItHykebjwB_Qe;+Z7RA4wV0b z7nwqS9&^n2%!kGS7%qk*2HuUpIrHCygqlufw;c)zpbpm@5n)H1B`Kb_tEC|e(+TgV z)2+Y=;r&82cbg-FQr3XE59XFjV2ba9bsR^P{OosA-uRZJ&8eK~f@^s6ipm(j_c&MYZep{uj!yDV&d=T)4SvK%Ih;EiMBF`QfCM z+{Cj6%<79X6*y}~2I~IcjLH7tZ(@c8tUJs06AgMo7#Z(1+*BV=-I=cGPhlqs*8-cXMVrO%q?3SdAhKL+6S-;$bGAR z2xrPF-_@>{8cBb(!7B5Y7(!`BdUeK;?Cq6tfD9~x1-TZv(x<9;190)DPMmc>;Wf_G zZNW(r$?7)6opDk=x>%bZ`+(f-lfOt}Nl)1!y%tbcfYSTejMMjpn);oO2Ijv?w|FU5G*;2%g7BmgSU5oP9evNl)M-Wz*Sf8 zLEbXvSYJ{X=|wRD2K?pnf}jS}n0-x>eLnySBQ4l`f8`H*1R-}ljrf}gvJ8K`k(Fuv zab(r-#!S&6bhgkic0AX7cT8y&%|nQF-=(!6T+;jh1i2VS=L4$-$r(|w74%_acz8`J zi4bI#8P)4ZJ{r7a*32Vjz*QQue@fK2&6@ex{4(P1MO$dyPD>VZu=hO$eH2h7wWYY3 zM3W+9jmT44ajZ7+tPudHPUrC&RKhGAub+_h6tD1sB$LP^idgg=+ghBjs3MtQG1rdMb&PofRWyDhmy?^rzw3oCW}cf$AtAJV$0D zDh}TCa$^hzR1@a*pu040r%w=Nd1?EUo7HBsj#H!+C(JqT%QeO;9y68FJjedJ*R{*= zuB9-n3k>ui?V2iO_^rx|Ux*4@fWWz5x~HG{so}bYW2!mu2bo;%uBWlB$iQ1hV;UUu z(l+rd!#BCg6B|O0IHvE)v4h7EKVeuQB0D?!~(?a`3kj4M5MDHF57G?DBjyW z(H{)Uz6-|sPrf--5tejqZb(ywIL5Ku)~x9m(ldep2#j)ix93fcAXqMN8~RWt5NiIl zqh|}KX-q>TWlvheMaJ)L@*QBLgB$(KbfJ})jcWRFxo=8$+GV(i7?YnU+dkBmF7D`r zaY)AJ82Zu~x6Zcrd{~i4*_eE~y+P=GD!6t`jamWb!*QDJ#iGp#2@)J^$Xm+$az6DR zhQTuGvS3Vck|I1zogam>)lXj3MOCcoZkvecSpf>U#%V&<;n=KhrV@FV^H9WW=A1=# zp=Cp{lgh`oJBp(E@8Ba3Z4L9(+ z%ok5_Cl*;UdJ|VztdmMzkJNe(M^b2XM)D99nTI|0 z6xd_Xs`5XSZ@wwqSo4H}Jw)j}H_ksgmTREOh!u++i32(I6mto#STll=WaC#dC_X1g z*h0ftUzm@V+K!vbC>jZh`Bq&XW)00xKx&mC$#Ov_@PUE()X*Rh$HPTMajA&r&D_%i ztV2`2fOg5KU%*{T#XyfL;j_OWM}K;Syj`y^#igyhk~DkD zEO?A_$ohX;dOg&FNVNF@(pWIT^dr)+>Gn~)lm!yTfyd$86X}|V#jjdO5n;%?bM{D} zonHQ)^{iuu3MEA@d1E`#K^!m#F+>!%&G(7)&u>bgubPYYKIgcQdMI$J}!)G52 z3H55-weU|3@WY#Xbqv@J&6YUm2|X#|W2ajKleP)idQi!7q>U30;|lYEL3Tz&^r@;sMr!B#Zz@UrHrj5&?o(oReABu`*ilhNvEj22U`@r7_URjJ%t@Eh7V)ZJN|(i&KECtO@H(I$RZDw_F-SaMg+36FZKl z)_`6>ucJ{8$_UErN2N_;*99Re!O1S*00;D^N1&*g%Yt)=!)^7X#t=-$P$d(~oyg5H zC4CCdjZf1U+Ock#FiNFB3fM9>reT5q09c`w-3gN7V8PcLL^eVGZ_Y2bDnuWDBEfet$4_|f;))JMRk1fusB|aHN$;z zZ!k%hHs@uOZp?cSKfPjFDo1*43%#~H@#r8$AUyUT+v{7l*JfCX%9IE|5?;qE73OG>ILfV7Xl&+@m3E`xLG(Y!Xcel93G>!V|byG*e&-3 zog;6x5rdGwiyob8UBqCkU^$g@g*nA=YK&z8R`=SZ`Rf+5xw(?&8>nTsbI&AcJprvC z1jH(zE_U@km21MLKm&?M2*ux5J*E6o2XZRPH9;Uf;l1&zeZ8E0eha>Bb)DZk7w~N>*Xbg(~ z0RBUUjQ1UlW<;JjY#QH8UNLN^1*0J*1Nes`eMdUF!bwWW8D(gk5;UA|O1ia@83o;< z8D-O^NQ3AsJt>t~^d1ca+GNozDa@>`j2~*!LmWsVg+N|fxoSSugnVDbW+fBC?iU9I zq#;y&YRe)5vr8u$=ub=W_A+0{7*btSqtu z6}1!dH$nBJBu|LCX5p?_9VEzcr_|A0T0s8D}GO9qRjgbdDfhgJ(Os=QZ0aPcusDS69jyqHJz6S*Bt1`eji!>Q_P5!Q%fIX^#7PxhiD z^f=9P%e!ZlaGW;8o|vVFCcr*^S>GK!X*pebVL2po{59#&72&BWtbWmf+>WXZd7UAQ zEK7_JE_s3LRFz!x#6^L8fr-XT9(CRz&IeP!r_!Z$X&6VZ85#BVrzpaO2pDSL{`IWI zZi|bObOLy18_^?z2BqJi?rM0$BzlWr9sZPYySs)1$T@AfrI%$QR>tK?!N>L$JIj}i zRRim73b@EV)XTkA_Aew0tfp|i$)NE2ZV2MsktN0s8Zm%?^iY4jVYgNUwN{uY&c&H` z7@@bBq!QvO6f5n->S@mWOLk4e5xVjD)`#IQZ*M%tRo4m9fIR**JHba z^BF?9Br^ko2WrgYJgkQIjH^w>(m&MPb}^|DcMUH_3m;=Iz~-osAH(Avpx?n2P%&4HlARzFcBY`AzByt0#Dh^oV_!PPTN1%mm9K9G0!nl$dS5m&B zIfXd~{psr}s)gH@kfnNc2D4ys%JLsd0-ZWx3O(okmk1#p~@mS|_4TQZ^j4{!6SED%K)FT! zCNw1+3urdh^T;=80me^%_O2CjK|#6ZS*h4qBjQL^n*fE*0ncArMY(Iz;Zcemnnu`r z3J@W*H!iJ`s18X3W;LYfs--m*Qa2grUepmn#g$bU1ddhZg-d7VIo+R~Dwn0M4iqZH zJM%O~C{=RV*|w%Uk)of-0ZL77Mt0kmYEqTMe8i~cJt!oeFmXtlZ400sME9xV8s^X@ zD&YZRN_!dWxamh5V6ZsB=c}bG$_5TOZhZ%>EW5cnw;9_VE0eAm>@sl20)!tnLEgMD zF{`Z*0BK{f)r?aG0m7?kIdvZ(^{ImWu`ZyH*kPV^Q@bk`3#1YN$1-cHXp9T4RA-Uj ze)P^v*is?_jmt^-I<+vEk(qE$p`%E^-1Y6wgapRpPWTz?QUVpX95O-FN#A~b>5a`z034o9If|B6 zND+zoh}dI%)l_;3UL;Y1G-85zABd-Old(Ug7=^wZ2?+%A9ZqR(k{m>g08`}!bDFhd zKKRV>%^Xg_k@XGx)NnP;(J@OP$RH3-*sVLFSA%fRgk7cb><@bEOAOKm)DHTAQL*~< zu9K5Di=OaNG*jzu0;aV`#h zM{3Qh4pT22AW~J=cIYuqnG}W`_Q78DFt}$XMF>jyNG*jQmpWt1#9_GKe=jNsJ{Ad7 zGsFl@*CS;$Qpp)B61Pr3RI)UaF^ml;PIf!dhy+Z0*k&67D_G9DXLrbkSkPl~%b$9F zlJV-0D;#qh4Ay$aNFfPSI3y0e>e%JtqO_h|ZdcotWR(o+F8EFgNDq~|Hq`b|>QTXU z^&1o3qP&GlmsZ#i74w3oyKKi;;*uMdqHqsv&~VbSg-{!1Bx3`eac6z_`*aPb6yl*| z*|kJBDh6w&IHrB=)yV4E08)o;{3JLy3^ixHNez@=6~|K1q`NpAwGOn#kAu3%Okm*a z-$?YPGlt4@&rmkSCE}&`(iClJ3Mm~=Gg4>S_*=q) z25nFcqgKf%J7>4{tyEB-D8hlKPio`JsqS_ZlN-)*Nkklh=GCTYg^aFM8|36qdI=_d zlbey!y37Ivhy#vnd(yxlY{Uc2xfVa-vT?wL0eN4|$Q;oYVUh?{8)WsMbATI$T!W8q zY8eiw%Sd+z0DrYW63)8x)xBAxW#kZk>)$lmjOVLnON)xb6j1W$YmoLQ?>}@ zTu90xIop^gb6ig}J z+v|$(@Z>~Sor@2`qXMm1508>bRL7=0X9ydcRXjFEZCGY}YAfkYju@@cTrOpAzHz9I z!1k_63$nj3PqryefU7LToJM6;DJhT zpx_an=8l5vqJXj0k1sBM)G;ZNDAfjZ=XL6PRMEQSb)18?{VMq1;n1vV6&P)fWYBzx zZf*E8nNl*$;}R*{IQH*T2>3*-A!8_dj02jn^^H6?I*B>YTA8p*DF@5QgPc@JYqHUp zQCgVG`Iw!~J?J>Gv;Ea1oU!OLT8fz- zBeej#;Aa}h{OL^;yg+r3Gq6*TMKzR23W4&CiRDZR@r2>;BW<`i{{XctQAC_5$RXK@ z$x+g}UoJxp$5Z!cqsk3gI@_7g_O2FT9vcHEao)39fq}6gWnV8Z_o9*75^&1ehnFLf zpfZgIN*z3_kuJ6@cOIgGWR{{xe9Tri7-8$)q88G`LI5PkBa$$GTGI>&5fF2peKASn z84HYp_|iz_fl??)Ljfx2U@@h4$fq4MuAmr$$dX6&sbyy30Nb8p+uoZTj&P@Pc@Tf4 zEQ)5tEsT-R9+lN!vQSO|+zR5{MkZ*MR1z-ak<-&W>#FS{>4X)*!!a9=YGX!+DIUX9 zbWk_17tlUjw>`NJdI5-2O6p_xh~|E^JF(rna|G6HG8+Lhz9l0_87rTd^rwKKxNf{) zMmcT2N}4##vqp=6r*i#9DfW+|Ib4)3U&=SwP@cDpTf0UMv(Kh(yb{EIRW3mBxM{8je&9j>eU763&`(bF&p*y)aKo>huZ%kTiuHg+#}j zIzneq+)^c#kZL}s)KNJR$FDkDJA=xDh?7P#FvK1ERRu!#33Bo*n2Rb2mM@+>b@le9 zG*e3yUlj^pGOh%Wu7XEGM^9>GR&hmv7{Thto+;M(DDj47(%BM$^LkdT&l%&l)0Kh@ z3=`X>LvwH*=H4}sX-3rLH_kQ-^{S@%nzP~m0L-WV0IWZ~dESW7jIm8pWC6&If|#y2 znAnl|pL*cuONajeGf_F@WlXB^Fmr)QaAsp7=Ky4Pr$h1|=Anc4U-H(O1UCy&04?(!lynrhzj-IH{^p*3VNwdBqq7sOiRMo)Zqz_4>0!7Br2zfod({^I0Ji&sbseWO-lntv0L)+i0Q(>R0FhLmko%wQQWD9kJe+4ZIPFl&32Uds zELKe|mfKGCpr5!uO5^;?{I$3Wk)~X5@Dpr;GIph{HF~A&$dg(dXasxXGHTmdd`Ne4k^(qoqeq`sBD&7#JY6K}6J?Zx!yMJ*+zdHL0 zsEFi&Vqg{BhB7?`aaLq($*3ar<&jzve82LVO~>ye{!vs^mlo2pvm!{#x|Z0FY9T0N z!v_qbY;vT{`^5hMGV9QZybPRMsxEF><6uI{OMX>{nCFg=|Q9| zv{1w~$(MD`4^v8HIMuhC*On>Q-@YHoQjd{;oYt#6RUpD|zBzA8*3952&Xc+4if=>h zOSSpt`hRK*&O#{3$?P#o1%b)kk1hFEWafQ^b$Oc8AqUD=R!Kc}q7jfdfk%{gqHo?M zB2V8MQ_{@qu#7UEcs+e;eNB`=m|gO5k@fv4@67)I*GgFZ)SmwUr4bBUE26WpD7=9< ZCY*Q2lK^s6I0Cu<0I - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/large_rock.urdf.xacro b/urdf/world/large_rock.urdf.xacro deleted file mode 100644 index 4a4cf34e0..000000000 --- a/urdf/world/large_rock.urdf.xacro +++ /dev/null @@ -1,23 +0,0 @@ - - - \ - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/medium_rock.urdf.xacro b/urdf/world/medium_rock.urdf.xacro deleted file mode 100644 index 4cd8551b4..000000000 --- a/urdf/world/medium_rock.urdf.xacro +++ /dev/null @@ -1,23 +0,0 @@ - - - \ - - - - - - - - - - - - - - - - - - diff --git a/urdf/world/small_rock.urdf.xacro b/urdf/world/small_rock.urdf.xacro deleted file mode 100644 index 609e15b0d..000000000 --- a/urdf/world/small_rock.urdf.xacro +++ /dev/null @@ -1,23 +0,0 @@ - - - \ - - - - - - - - - - - - - - - - - - From 47a8ec8e844b00dd068f1659e4a02a815aef534b Mon Sep 17 00:00:00 2001 From: Vishal Date: Sun, 22 Feb 2026 13:53:57 -0500 Subject: [PATCH 06/31] Added angle to rover threshold for stopping --- config/navigation.yaml | 1 + navigation/approach_target.py | 22 +++++++++++++++++++--- navigation/context.py | 3 ++- navigation/nav.py | 1 + 4 files changed, 23 insertions(+), 4 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index 78234eb78..8f74f37b8 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -68,6 +68,7 @@ navigation: angle_thresh: 0.0872665 #pi/36 radians / 5 degrees distance_threshold: 1.0 distance_look_threshold: 5.0 + stop_angle_threshold: 0.2094 #pi/15 radians / 12 degrees update_delay: 3.0 single_tag: diff --git a/navigation/approach_target.py b/navigation/approach_target.py index a2f4585a2..2fbd8bd8d 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -1,4 +1,5 @@ import numpy as np +import math from typing import Any from navigation.trajectory import Trajectory from navigation.astar import AStar, NoPath, OutOfBounds @@ -20,6 +21,7 @@ class ApproachTargetState(State): USE_COSTMAP: bool DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float + STOP_ANGLE_THRESHOLD:float time_begin: Time astar_traj: Trajectory target_traj: Trajectory @@ -50,6 +52,7 @@ 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.STOP_ANGLE_THRESHOLD = context.node.get_parameter("search.stop_angle_threshold").value self.COST_INFLATION_RADIUS = context.node.get_parameter("costmap.initial_inflation_radius").value self.marker_pub = context.node.create_publisher(Marker, "target_trajectory", 10) self.astar_traj = Trajectory(np.array([])) @@ -224,6 +227,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self if self.self_in_distance_threshold(context, self.object_type): + context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) arrived = False cmd_vel = Twist() @@ -263,6 +267,8 @@ def on_loop_costmap_enabled(self, context: Context) -> State: 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 + context.node.get_logger().info("Exited without distance threshold") + self.self_in_distance_threshold(context, self.object_type) return self.next_state(context=context, is_finished=True) return self @@ -397,12 +403,22 @@ def self_in_distance_threshold(self, context: Context, object_type: int): if time_diff is None: return False rover_translation = rover_SE3.translation()[0:2] - distance_to_target = d_calc(rover_translation, tuple(target_pos)) + distance_to_target = d_calc(rover_translation, tuple(target_pos)) if(object_type in self.no_look_ahead_dict.values()): return distance_to_target < self.DISTANCE_THRESHOLD else: - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD and time_diff < Duration(nanoseconds=50000000) - + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD and time_diff < Duration(nanoseconds=30000000) and self.target_in_frame(context, rover_SE3, target_pos) + + def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): + rover_to_model = target_pos - rover_SE3.translation() + rover_norm_model = np.linalg.norm(rover_to_model) + rover_to_model /= rover_norm_model + rover_forward = rover_SE3.rotation()[:, 0] + rover_dot_model = np.dot(rover_to_model, rover_forward) + angle_to_model = np.arccos(rover_dot_model) + angle_to_model = math.copysign(angle_to_model, np.cross(rover_forward, rover_to_model)[2]) + context.node.get_logger().info("Angle to model" + str(angle_to_model)) + return angle_to_model < self.STOP_ANGLE_THRESHOLD and angle_to_model > -1 * self.STOP_ANGLE_THRESHOLD def point_in_distance_threshold(self, context: Context, point): if point is None: return False diff --git a/navigation/context.py b/navigation/context.py index f6e4804ec..aae029369 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -105,7 +105,7 @@ def get_target_position(self, frame: str) -> np.ndarray | None: return target_pose.translation() - def get_time_diff(self, frame: str) -> None | Time: + def get_time_diff(self, frame: str) -> None | Duration: try: waste, t = SE3.from_tf_tree_with_time(self.ctx.tf_buffer, frame, self.ctx.world_frame) except ( @@ -477,6 +477,7 @@ def setup(self, node: Node): def enable_auton(self, request: EnableAuton.Request, response: EnableAuton.Response) -> EnableAuton.Response: self.node.get_logger().info("Received new course to navigate!") + self.node.get_logger().info(str(request.waypoints[0].type)) if request.enable: ref_point = np.array( [ diff --git a/navigation/nav.py b/navigation/nav.py index d051af46f..2f03401e5 100755 --- a/navigation/nav.py +++ b/navigation/nav.py @@ -83,6 +83,7 @@ def __init__(self, ctx: Context) -> None: ("search.angle_thresh", Parameter.Type.DOUBLE), ("search.distance_threshold", Parameter.Type.DOUBLE), ("search.distance_look_threshold", Parameter.Type.DOUBLE), + ("search.stop_angle_threshold", Parameter.Type.DOUBLE), # Image Targets ("image_targets.increment_weight", Parameter.Type.INTEGER), ("image_targets.decrement_weight", Parameter.Type.INTEGER), From 97abb38d1eda1bd80c5e10d757e58ee0413ca7cf Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 12 Mar 2026 19:20:13 -0400 Subject: [PATCH 07/31] Added end point and working towards adding trajectory points when reaching bad stopping --- navigation/approach_target.py | 21 ++++++++++++++------- navigation/trajectory.py | 3 +++ 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 2fbd8bd8d..ca2825bfd 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -19,6 +19,7 @@ class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool + SPIN_ROVER: bool DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float STOP_ANGLE_THRESHOLD:float @@ -50,6 +51,7 @@ def on_enter(self, context: Context) -> None: return self.USE_COSTMAP = context.node.get_parameter("costmap.use_costmap").value or current_waypoint.enable_costmap + self.SPIN_ROVER = False 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.STOP_ANGLE_THRESHOLD = context.node.get_parameter("search.stop_angle_threshold").value @@ -167,7 +169,9 @@ 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 self.SPIN_ROVER: + self.target_traj # 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") @@ -182,9 +186,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(): @@ -226,9 +228,11 @@ def on_loop_costmap_enabled(self, context: Context) -> State: context.node.get_logger().info("Found low-cost point") return self + # If we are within the distance threshold of the target we have finished if self.self_in_distance_threshold(context, self.object_type): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) + arrived = False cmd_vel = Twist() if not self.astar_traj.done(): @@ -260,8 +264,6 @@ 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 - # Otherwise we need to dilate to get closer else: context.node.get_logger().info("Too far from target, dilating costmap") @@ -269,7 +271,11 @@ def on_loop_costmap_enabled(self, context: Context) -> State: # Fully dilated and still failed, go to next state context.node.get_logger().info("Exited without distance threshold") self.self_in_distance_threshold(context, self.object_type) - return self.next_state(context=context, is_finished=True) + if not self.SPIN_ROVER: + self.SPIN_ROVER = True + else: + self.next_state(context, is_finished=True) + #return self.next_state(context=context, is_finished=True) return self else: @@ -419,6 +425,7 @@ def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): angle_to_model = math.copysign(angle_to_model, np.cross(rover_forward, rover_to_model)[2]) context.node.get_logger().info("Angle to model" + str(angle_to_model)) return angle_to_model < self.STOP_ANGLE_THRESHOLD and angle_to_model > -1 * self.STOP_ANGLE_THRESHOLD + def point_in_distance_threshold(self, context: Context, point): if point is None: return False diff --git a/navigation/trajectory.py b/navigation/trajectory.py index 84cb8d255..5b406bf14 100644 --- a/navigation/trajectory.py +++ b/navigation/trajectory.py @@ -27,6 +27,9 @@ def decerement_point(self) -> bool: """ self.cur_pt = max(0, self.cur_pt - 1) return self.cur_pt <= 0 + def add_end_point(self, position: list) -> int: + self.coordinates.append(position) + return len(self.coordinates) - 1 def done(self) -> bool: return self.cur_pt >= len(self.coordinates) From efc2ef2cc1b65174edf29b176af941efab9f36e4 Mon Sep 17 00:00:00 2001 From: Vishal Date: Tue, 24 Mar 2026 18:06:45 -0400 Subject: [PATCH 08/31] Added points to trajectory --- navigation/approach_target.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index ca2825bfd..80098f219 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -171,7 +171,8 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self if self.SPIN_ROVER: - self.target_traj + self.target_traj.add_end_point(rover_pose.translation() - rover_pose.rotation()[:, 0]) + self.target_traj.add_end_point(self.target_position[0:2]) # 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") From 6796b09ee8ec8b493a6e029211671eeb4e3bd509 Mon Sep 17 00:00:00 2001 From: Vishal Date: Tue, 31 Mar 2026 19:22:16 -0400 Subject: [PATCH 09/31] Adding trajectory points and scrapped using trajectory, instead using individual cmd_vels to move in approach target --- navigation/approach_target.py | 38 +++++++++++++++++++++++++++-------- navigation/context.py | 1 - navigation/trajectory.py | 11 ++++++++-- 3 files changed, 39 insertions(+), 11 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 80098f219..35fc83332 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -169,10 +169,24 @@ 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 self.SPIN_ROVER: - self.target_traj.add_end_point(rover_pose.translation() - rover_pose.rotation()[:, 0]) - self.target_traj.add_end_point(self.target_position[0:2]) + if self.SPIN_ROVER is None or self.SPIN_ROVER: + if self.SPIN_ROVER is not None: + cmd_vel_func, arrived_func = self.spin_rover_drive(context, rover_pose.translation() - rover_pose.rotation()[:, 0]) + if(arrived_func): + self.SPIN_ROVER = None + else: + context.rover.send_drive_command(cmd_vel_func) + else: + cmd_vel_func, arrived_func = self.spin_rover_drive(context, rover_pose.translation(),self.target_position) + if self.self_in_distance_threshold(context, self.object_type): + context.node.get_logger().info("Exited through distance threshold") + return self.next_state(context=context, is_finished=True) + if(arrived_func): + context.node.get_logger().info("Exited without distance threshold") + self.next_state(context, True) + else: + context.rover.send_drive_command(cmd_vel_func) + 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") @@ -255,7 +269,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() @@ -271,11 +284,10 @@ def on_loop_costmap_enabled(self, context: Context) -> State: if not context.shrink_dilation(): # Fully dilated and still failed, go to next state context.node.get_logger().info("Exited without distance threshold") - self.self_in_distance_threshold(context, self.object_type) - if not self.SPIN_ROVER: + if self.SPIN_ROVER is not None and not self.SPIN_ROVER: self.SPIN_ROVER = True else: - self.next_state(context, is_finished=True) + return self.next_state(context, is_finished=True) #return self.next_state(context=context, is_finished=True) return self @@ -397,6 +409,16 @@ def display_markers(self, context: Context): context.publish_path_marker( points=np.array([self.target_position]), color=[1.0, 1.0, 0.0], ns=str(type(self)) ) + def spin_rover_drive(self, context: Context, phase_1: np.ndarray, phase_2: np.ndarray = None): + cmd_vel, arrived = context.drive.get_drive_command( + phase_2 if phase_2 is not None else phase_1, + context.rover.get_pose_in_map(), + context.node.get_parameter("single_tag.stop_threshold").value / 10, + context.node.get_parameter("waypoint.drive_forward_threshold").value / 5, + False if phase_2 is not None else True + ) + return cmd_vel, arrived + def self_in_distance_threshold(self, context: Context, object_type: int): rover_SE3 = context.rover.get_pose_in_map() diff --git a/navigation/context.py b/navigation/context.py index de09604b8..21ce9ed43 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -482,7 +482,6 @@ def setup(self, node: Node): def enable_auton(self, request: EnableAuton.Request, response: EnableAuton.Response) -> EnableAuton.Response: self.node.get_logger().info("Received new course to navigate!") - self.node.get_logger().info(str(request.waypoints[0].type)) if request.enable: ref_point = np.array( [ diff --git a/navigation/trajectory.py b/navigation/trajectory.py index 5b406bf14..5a57dae1b 100644 --- a/navigation/trajectory.py +++ b/navigation/trajectory.py @@ -27,8 +27,15 @@ def decerement_point(self) -> bool: """ self.cur_pt = max(0, self.cur_pt - 1) return self.cur_pt <= 0 - def add_end_point(self, position: list) -> int: - self.coordinates.append(position) + + def add_end_point(self, position: np.ndarray) -> int: + """ + Adds point to end of trajectory, returns index of end point added + """ + if(len(self.coordinates)) == 0: + self.coordinates = np.atleast_2d(position) + else: + self.coordinates = np.vstack((self.coordinates, position)) return len(self.coordinates) - 1 def done(self) -> bool: From cb194fbd240e7bcc0cb14ded2324282273131884 Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 2 Apr 2026 20:01:39 -0400 Subject: [PATCH 10/31] Backup appears to be working --- navigation/approach_target.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 35fc83332..0933f44c4 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -29,6 +29,7 @@ class ApproachTargetState(State): astar: AStar time_last_updated: Time target_position: np.ndarray | None + fixed_position: np.ndarray | None marker_timer: Timer update_timer: Timer object_type: int @@ -61,6 +62,7 @@ def on_enter(self, context: Context) -> None: self.target_traj = Trajectory(np.array([])) self.astar = AStar(context=context) self.target_position = None + self.fixed_position = None self.time_last_updated = context.node.get_clock().now() self.time_begin = context.node.get_clock().now() self.object_type = current_waypoint.type.val @@ -170,8 +172,10 @@ def on_loop_costmap_enabled(self, context: Context) -> State: context.rover.send_drive_command(Twist()) return self if self.SPIN_ROVER is None or self.SPIN_ROVER: + if self.fixed_position is None: + self.fixed_position = rover_pose.translation() - rover_pose.rotation()[:, 0] + np.array([1e-8, 0, 0]) if self.SPIN_ROVER is not None: - cmd_vel_func, arrived_func = self.spin_rover_drive(context, rover_pose.translation() - rover_pose.rotation()[:, 0]) + cmd_vel_func, arrived_func = self.spin_rover_drive(context, self.fixed_position) if(arrived_func): self.SPIN_ROVER = None else: @@ -410,11 +414,15 @@ def display_markers(self, context: Context): points=np.array([self.target_position]), color=[1.0, 1.0, 0.0], ns=str(type(self)) ) def spin_rover_drive(self, context: Context, phase_1: np.ndarray, phase_2: np.ndarray = None): + context.node.get_logger().info("rover_dir" + str(context.rover.get_pose_in_map().rotation()[:, 0][:2] * -1)) + context.node.get_logger().info("target_dir" + str((phase_1 - context.rover.get_pose_in_map().translation())[:2])) cmd_vel, arrived = context.drive.get_drive_command( phase_2 if phase_2 is not None else phase_1, context.rover.get_pose_in_map(), - context.node.get_parameter("single_tag.stop_threshold").value / 10, - context.node.get_parameter("waypoint.drive_forward_threshold").value / 5, + context.node.get_parameter("single_tag.stop_threshold").value + if phase_2 is not None else context.node.get_parameter("single_tag.stop_threshold").value / 10, + context.node.get_parameter("waypoint.drive_forward_threshold").value + if phase_2 is not None else context.node.get_parameter("backup.drive_forward_threshold").value, False if phase_2 is not None else True ) return cmd_vel, arrived @@ -444,6 +452,7 @@ def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): rover_to_model /= rover_norm_model rover_forward = rover_SE3.rotation()[:, 0] rover_dot_model = np.dot(rover_to_model, rover_forward) + context.node.get_logger().info("Rover to model: " + str(rover_to_model) + ", rover_forward: " + str(rover_forward)) angle_to_model = np.arccos(rover_dot_model) angle_to_model = math.copysign(angle_to_model, np.cross(rover_forward, rover_to_model)[2]) context.node.get_logger().info("Angle to model" + str(angle_to_model)) From 881e847aa9798193fddefe6d176aa4bfb85ff7c9 Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 9 Apr 2026 19:11:48 -0400 Subject: [PATCH 11/31] Removed logging info and fixed error --- navigation/approach_target.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 0933f44c4..fa6ce9b92 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -187,7 +187,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self.next_state(context=context, is_finished=True) if(arrived_func): context.node.get_logger().info("Exited without distance threshold") - self.next_state(context, True) + return self.next_state(context, True) else: context.rover.send_drive_command(cmd_vel_func) return self @@ -414,8 +414,6 @@ def display_markers(self, context: Context): points=np.array([self.target_position]), color=[1.0, 1.0, 0.0], ns=str(type(self)) ) def spin_rover_drive(self, context: Context, phase_1: np.ndarray, phase_2: np.ndarray = None): - context.node.get_logger().info("rover_dir" + str(context.rover.get_pose_in_map().rotation()[:, 0][:2] * -1)) - context.node.get_logger().info("target_dir" + str((phase_1 - context.rover.get_pose_in_map().translation())[:2])) cmd_vel, arrived = context.drive.get_drive_command( phase_2 if phase_2 is not None else phase_1, context.rover.get_pose_in_map(), @@ -452,10 +450,8 @@ def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): rover_to_model /= rover_norm_model rover_forward = rover_SE3.rotation()[:, 0] rover_dot_model = np.dot(rover_to_model, rover_forward) - context.node.get_logger().info("Rover to model: " + str(rover_to_model) + ", rover_forward: " + str(rover_forward)) angle_to_model = np.arccos(rover_dot_model) angle_to_model = math.copysign(angle_to_model, np.cross(rover_forward, rover_to_model)[2]) - context.node.get_logger().info("Angle to model" + str(angle_to_model)) return angle_to_model < self.STOP_ANGLE_THRESHOLD and angle_to_model > -1 * self.STOP_ANGLE_THRESHOLD def point_in_distance_threshold(self, context: Context, point): From 6f9f9e9532f7a1519197ce3b61bcff13468c93f7 Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 9 Apr 2026 19:13:53 -0400 Subject: [PATCH 12/31] Style fixes --- navigation/approach_target.py | 55 +++++++++++++++++++++-------------- navigation/context.py | 6 ++-- navigation/trajectory.py | 2 +- 3 files changed, 37 insertions(+), 26 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index fa6ce9b92..3f827160d 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -22,7 +22,7 @@ class ApproachTargetState(State): SPIN_ROVER: bool DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float - STOP_ANGLE_THRESHOLD:float + STOP_ANGLE_THRESHOLD: float time_begin: Time astar_traj: Trajectory target_traj: Trajectory @@ -66,7 +66,7 @@ def on_enter(self, context: Context) -> None: self.time_last_updated = context.node.get_clock().now() self.time_begin = context.node.get_clock().now() self.object_type = current_waypoint.type.val - self.no_look_ahead_dict = {'NO_SEARCH': 0, 'POST': 1} + self.no_look_ahead_dict = {"NO_SEARCH": 0, "POST": 1} self.marker_timer = context.node.create_timer( context.node.get_parameter("pub_path_rate").value, lambda: self.display_markers(context=context) @@ -176,16 +176,18 @@ def on_loop_costmap_enabled(self, context: Context) -> State: self.fixed_position = rover_pose.translation() - rover_pose.rotation()[:, 0] + np.array([1e-8, 0, 0]) if self.SPIN_ROVER is not None: cmd_vel_func, arrived_func = self.spin_rover_drive(context, self.fixed_position) - if(arrived_func): + if arrived_func: self.SPIN_ROVER = None else: context.rover.send_drive_command(cmd_vel_func) else: - cmd_vel_func, arrived_func = self.spin_rover_drive(context, rover_pose.translation(),self.target_position) + cmd_vel_func, arrived_func = self.spin_rover_drive( + context, rover_pose.translation(), self.target_position + ) if self.self_in_distance_threshold(context, self.object_type): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - if(arrived_func): + if arrived_func: context.node.get_logger().info("Exited without distance threshold") return self.next_state(context, True) else: @@ -205,7 +207,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") - if(self.target_traj.increment_point()): + if self.target_traj.increment_point(): break if not self.target_traj.done(): @@ -246,12 +248,12 @@ def on_loop_costmap_enabled(self, context: Context) -> State: else: context.node.get_logger().info("Found low-cost point") return self - + # If we are within the distance threshold of the target we have finished if self.self_in_distance_threshold(context, self.object_type): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - + arrived = False cmd_vel = Twist() if not self.astar_traj.done(): @@ -262,7 +264,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State: context.node.get_parameter("single_tag.stop_threshold").value, context.node.get_parameter("waypoint.drive_forward_threshold").value, ) - # If we have arrived increment the a-star trajectory if arrived: @@ -292,7 +293,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: self.SPIN_ROVER = True else: return self.next_state(context, is_finished=True) - #return self.next_state(context=context, is_finished=True) + # return self.next_state(context=context, is_finished=True) return self else: @@ -413,19 +414,25 @@ def display_markers(self, context: Context): context.publish_path_marker( points=np.array([self.target_position]), color=[1.0, 1.0, 0.0], ns=str(type(self)) ) + def spin_rover_drive(self, context: Context, phase_1: np.ndarray, phase_2: np.ndarray = None): cmd_vel, arrived = context.drive.get_drive_command( - phase_2 if phase_2 is not None else phase_1, - context.rover.get_pose_in_map(), - context.node.get_parameter("single_tag.stop_threshold").value - if phase_2 is not None else context.node.get_parameter("single_tag.stop_threshold").value / 10, - context.node.get_parameter("waypoint.drive_forward_threshold").value - if phase_2 is not None else context.node.get_parameter("backup.drive_forward_threshold").value, - False if phase_2 is not None else True - ) + phase_2 if phase_2 is not None else phase_1, + context.rover.get_pose_in_map(), + ( + context.node.get_parameter("single_tag.stop_threshold").value + if phase_2 is not None + else context.node.get_parameter("single_tag.stop_threshold").value / 10 + ), + ( + context.node.get_parameter("waypoint.drive_forward_threshold").value + if phase_2 is not None + else context.node.get_parameter("backup.drive_forward_threshold").value + ), + False if phase_2 is not None else True, + ) return cmd_vel, arrived - def self_in_distance_threshold(self, context: Context, object_type: int): rover_SE3 = context.rover.get_pose_in_map() if rover_SE3 is None: @@ -439,10 +446,14 @@ def self_in_distance_threshold(self, context: Context, object_type: int): return False rover_translation = rover_SE3.translation()[0:2] distance_to_target = d_calc(rover_translation, tuple(target_pos)) - if(object_type in self.no_look_ahead_dict.values()): + if object_type in self.no_look_ahead_dict.values(): return distance_to_target < self.DISTANCE_THRESHOLD else: - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD and time_diff < Duration(nanoseconds=30000000) and self.target_in_frame(context, rover_SE3, target_pos) + return ( + distance_to_target < self.LOOK_DISTANCE_THRESHOLD + and time_diff < Duration(nanoseconds=30000000) + and self.target_in_frame(context, rover_SE3, target_pos) + ) def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): rover_to_model = target_pos - rover_SE3.translation() @@ -453,7 +464,7 @@ def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): angle_to_model = np.arccos(rover_dot_model) angle_to_model = math.copysign(angle_to_model, np.cross(rover_forward, rover_to_model)[2]) return angle_to_model < self.STOP_ANGLE_THRESHOLD and angle_to_model > -1 * self.STOP_ANGLE_THRESHOLD - + def point_in_distance_threshold(self, context: Context, point): if point is None: return False diff --git a/navigation/context.py b/navigation/context.py index 21ce9ed43..d52fbc65f 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -104,7 +104,7 @@ def get_target_position(self, frame: str) -> np.ndarray | None: return None return target_pose.translation() - + def get_time_diff(self, frame: str) -> None | Duration: try: waste, t = SE3.from_tf_tree_with_time(self.ctx.tf_buffer, frame, self.ctx.world_frame) @@ -114,7 +114,7 @@ def get_time_diff(self, frame: str) -> None | Duration: tf2_ros.ExtrapolationException, ): return None - + now = self.ctx.node.get_clock().now() time = Time.from_msg(t) return now - time @@ -133,7 +133,7 @@ def current_target_pos(self) -> np.ndarray | None: return self.get_target_position("pick") case _: return None - + def current_time_diff(self): assert self.ctx.course is not None diff --git a/navigation/trajectory.py b/navigation/trajectory.py index 5a57dae1b..f21351ed7 100644 --- a/navigation/trajectory.py +++ b/navigation/trajectory.py @@ -32,7 +32,7 @@ def add_end_point(self, position: np.ndarray) -> int: """ Adds point to end of trajectory, returns index of end point added """ - if(len(self.coordinates)) == 0: + if (len(self.coordinates)) == 0: self.coordinates = np.atleast_2d(position) else: self.coordinates = np.vstack((self.coordinates, position)) From a5f8d428fa1ad03bb2847ede8a722a34a31a8c7f Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 9 Apr 2026 19:23:28 -0400 Subject: [PATCH 13/31] Type hinted none to fix errors --- navigation/approach_target.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 3f827160d..456a58fd3 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -19,7 +19,7 @@ class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool - SPIN_ROVER: bool + SPIN_ROVER: bool | None DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float STOP_ANGLE_THRESHOLD: float @@ -415,7 +415,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 spin_rover_drive(self, context: Context, phase_1: np.ndarray, phase_2: np.ndarray = None): + def spin_rover_drive(self, context: Context, phase_1: np.ndarray, phase_2: np.ndarray | None = None): cmd_vel, arrived = context.drive.get_drive_command( phase_2 if phase_2 is not None else phase_1, context.rover.get_pose_in_map(), From 294516d52e4dda1fb87db12a8196d1789040467d Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 9 Apr 2026 20:24:01 -0400 Subject: [PATCH 14/31] Changed none bool to enum --- navigation/approach_target.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 456a58fd3..621ed4fdb 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -1,5 +1,6 @@ import numpy as np import math +from enum import Enum from typing import Any from navigation.trajectory import Trajectory from navigation.astar import AStar, NoPath, OutOfBounds @@ -15,11 +16,14 @@ from rclpy.duration import Duration from navigation.coordinate_utils import is_high_cost_point, d_calc, segment_path, cartesian_to_ij - +class SpinRoverVal(Enum): + NO_SPIN = 0 + BACKWARD = 1 + FORWARD = 2 class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool - SPIN_ROVER: bool | None + SPIN_ROVER: SpinRoverVal DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float STOP_ANGLE_THRESHOLD: float @@ -51,8 +55,8 @@ def on_enter(self, context: Context) -> None: if current_waypoint is None: return + self.SPIN_ROVER = SpinRoverVal.NO_SPIN self.USE_COSTMAP = context.node.get_parameter("costmap.use_costmap").value or current_waypoint.enable_costmap - self.SPIN_ROVER = False 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.STOP_ANGLE_THRESHOLD = context.node.get_parameter("search.stop_angle_threshold").value @@ -171,13 +175,13 @@ 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 self.SPIN_ROVER is None or self.SPIN_ROVER: + if self.SPIN_ROVER == SpinRoverVal.BACKWARD or self.SPIN_ROVER == SpinRoverVal.FORWARD: if self.fixed_position is None: self.fixed_position = rover_pose.translation() - rover_pose.rotation()[:, 0] + np.array([1e-8, 0, 0]) - if self.SPIN_ROVER is not None: + if self.SPIN_ROVER == SpinRoverVal.BACKWARD: cmd_vel_func, arrived_func = self.spin_rover_drive(context, self.fixed_position) if arrived_func: - self.SPIN_ROVER = None + self.SPIN_ROVER = SpinRoverVal.FORWARD else: context.rover.send_drive_command(cmd_vel_func) else: @@ -289,8 +293,8 @@ def on_loop_costmap_enabled(self, context: Context) -> State: if not context.shrink_dilation(): # Fully dilated and still failed, go to next state context.node.get_logger().info("Exited without distance threshold") - if self.SPIN_ROVER is not None and not self.SPIN_ROVER: - self.SPIN_ROVER = True + if self.SPIN_ROVER != SpinRoverVal.BACKWARD and self.SPIN_ROVER != SpinRoverVal.FORWARD: + self.SPIN_ROVER = SpinRoverVal.BACKWARD else: return self.next_state(context, is_finished=True) # return self.next_state(context=context, is_finished=True) From 419a277c5086270039c963ac6472d2b7497214f6 Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 9 Apr 2026 21:46:25 -0400 Subject: [PATCH 15/31] Style fixes part 2 --- navigation/approach_target.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 621ed4fdb..91632d844 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -16,10 +16,13 @@ from rclpy.duration import Duration from navigation.coordinate_utils import is_high_cost_point, d_calc, segment_path, cartesian_to_ij + class SpinRoverVal(Enum): NO_SPIN = 0 BACKWARD = 1 FORWARD = 2 + + class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool From 79139ec7c51c9f5b935a333f036ff913b7bfd4fd Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 16 Apr 2026 18:34:21 -0400 Subject: [PATCH 16/31] Switch to main mac --- navigation/approach_target.py | 119 +++++++++++++++------------------- navigation/trajectory.py | 10 --- 2 files changed, 51 insertions(+), 78 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 91632d844..c55729a12 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -1,7 +1,6 @@ import numpy as np import math -from enum import Enum -from typing import Any +from typing import Any, Literal from navigation.trajectory import Trajectory from navigation.astar import AStar, NoPath, OutOfBounds from . import costmap_search, stuck_recovery, waypoint, backup, state @@ -9,24 +8,18 @@ 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 from navigation.coordinate_utils import is_high_cost_point, d_calc, segment_path, cartesian_to_ij -class SpinRoverVal(Enum): - NO_SPIN = 0 - BACKWARD = 1 - FORWARD = 2 class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool - SPIN_ROVER: SpinRoverVal + WITHIN_DIST: bool DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float STOP_ANGLE_THRESHOLD: float @@ -36,7 +29,6 @@ class ApproachTargetState(State): astar: AStar time_last_updated: Time target_position: np.ndarray | None - fixed_position: np.ndarray | None marker_timer: Timer update_timer: Timer object_type: int @@ -58,18 +50,16 @@ def on_enter(self, context: Context) -> None: if current_waypoint is None: return - self.SPIN_ROVER = SpinRoverVal.NO_SPIN 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.STOP_ANGLE_THRESHOLD = context.node.get_parameter("search.stop_angle_threshold").value - self.COST_INFLATION_RADIUS = context.node.get_parameter("costmap.initial_inflation_radius").value - self.marker_pub = context.node.create_publisher(Marker, "target_trajectory", 10) self.astar_traj = Trajectory(np.array([])) self.target_traj = Trajectory(np.array([])) self.astar = AStar(context=context) self.target_position = None self.fixed_position = None + self.WITHIN_DIST = False self.time_last_updated = context.node.get_clock().now() self.time_begin = context.node.get_clock().now() self.object_type = current_waypoint.type.val @@ -178,27 +168,15 @@ 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 self.SPIN_ROVER == SpinRoverVal.BACKWARD or self.SPIN_ROVER == SpinRoverVal.FORWARD: - if self.fixed_position is None: - self.fixed_position = rover_pose.translation() - rover_pose.rotation()[:, 0] + np.array([1e-8, 0, 0]) - if self.SPIN_ROVER == SpinRoverVal.BACKWARD: - cmd_vel_func, arrived_func = self.spin_rover_drive(context, self.fixed_position) - if arrived_func: - self.SPIN_ROVER = SpinRoverVal.FORWARD - else: - context.rover.send_drive_command(cmd_vel_func) - else: - cmd_vel_func, arrived_func = self.spin_rover_drive( - context, rover_pose.translation(), self.target_position - ) - if self.self_in_distance_threshold(context, self.object_type): - context.node.get_logger().info("Exited through distance threshold") - return self.next_state(context=context, is_finished=True) - if arrived_func: - context.node.get_logger().info("Exited without distance threshold") - return self.next_state(context, True) - else: - context.rover.send_drive_command(cmd_vel_func) + if self.WITHIN_DIST: + cmd_vel, arrived = self.spin_rover(context, self.target_position) + if(arrived): + return costmap_search.CostmapSearchState() + context.rover.send_drive_command(cmd_vel) + #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD + if(all(self.self_in_distance_threshold(context, self.object_type))): + context.node.get_logger().info("Exited through distance threshold") + return self.next_state(context, True) return self # If the target trajectory is empty, develop a new path to it if len(self.target_traj.coordinates) == 0: @@ -257,9 +235,15 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self # If we are within the distance threshold of the target we have finished - if self.self_in_distance_threshold(context, self.object_type): + self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.object_type) + if(self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST): + context.node.get_logger().info("Exited through distance threshold") + return self.next_state(context=context, is_finished=True) + elif(self.object_type not in self.no_look_ahead_dict.values() and within_frame): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) + elif(self.WITHIN_DIST): + context.node.get_logger().info("Object within distance but not within frame") arrived = False cmd_vel = Twist() @@ -296,10 +280,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: if not context.shrink_dilation(): # Fully dilated and still failed, go to next state context.node.get_logger().info("Exited without distance threshold") - if self.SPIN_ROVER != SpinRoverVal.BACKWARD and self.SPIN_ROVER != SpinRoverVal.FORWARD: - self.SPIN_ROVER = SpinRoverVal.BACKWARD - else: - return self.next_state(context, is_finished=True) + return self.next_state(context, is_finished=True) # return self.next_state(context=context, is_finished=True) return self @@ -307,7 +288,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: context.rover.send_drive_command(cmd_vel) return self - + #Fix disabled state soon def on_loop_costmap_disabled(self, context: Context) -> State: from .long_range import LongRangeState @@ -316,6 +297,17 @@ def on_loop_costmap_disabled(self, context: Context) -> State: if self.target_position is None: return self + + if(self.WITHIN_DIST): + cmd_vel, arrived = self.spin_rover(context, self.target_position) + if(arrived): + return costmap_search.CostmapSearchState() + context.rover.send_drive_command(cmd_vel) + #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD + if(all(self.self_in_distance_threshold(context, self.object_type))): + context.node.get_logger().info("Exited through distance threshold") + return self.next_state(context, True) + return self arrived = False cmd_vel = Twist() @@ -325,8 +317,15 @@ 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 self.self_in_distance_threshold(context, self.object_type): + self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.object_type) + if(self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST): + context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) + elif(self.object_type not in self.no_look_ahead_dict.values() and within_frame): + context.node.get_logger().info("Exited through distance threshold") + return self.next_state(context=context, is_finished=True) + elif(self.WITHIN_DIST): + context.node.get_logger().info("Object within distance but not within frame") if arrived: if isinstance(self, LongRangeState): self.target_position = self.get_target_position(context) @@ -421,46 +420,29 @@ def display_markers(self, context: Context): context.publish_path_marker( points=np.array([self.target_position]), color=[1.0, 1.0, 0.0], ns=str(type(self)) ) - - def spin_rover_drive(self, context: Context, phase_1: np.ndarray, phase_2: np.ndarray | None = None): + def spin_rover(self, context: Context, target_pos: np.ndarray): cmd_vel, arrived = context.drive.get_drive_command( - phase_2 if phase_2 is not None else phase_1, - context.rover.get_pose_in_map(), - ( - context.node.get_parameter("single_tag.stop_threshold").value - if phase_2 is not None - else context.node.get_parameter("single_tag.stop_threshold").value / 10 - ), - ( - context.node.get_parameter("waypoint.drive_forward_threshold").value - if phase_2 is not None - else context.node.get_parameter("backup.drive_forward_threshold").value - ), - False if phase_2 is not None else True, - ) + target_pos, context.rover.get_pose_in_map(), + self.DISTANCE_THRESHOLD, 1e-8) return cmd_vel, arrived - - def self_in_distance_threshold(self, context: Context, object_type: int): + def self_in_distance_threshold(self, context: Context, object_type: int) -> tuple[bool | bool]: rover_SE3 = context.rover.get_pose_in_map() if rover_SE3 is None: - return False + return False, False target_pos = context.env.current_target_pos() if target_pos is None: - return False + return False, False time_diff = context.env.current_time_diff() if time_diff is None: - return False + return False, False rover_translation = rover_SE3.translation()[0:2] distance_to_target = d_calc(rover_translation, tuple(target_pos)) if object_type in self.no_look_ahead_dict.values(): - return distance_to_target < self.DISTANCE_THRESHOLD + return distance_to_target < self.DISTANCE_THRESHOLD, False else: - return ( - distance_to_target < self.LOOK_DISTANCE_THRESHOLD - and time_diff < Duration(nanoseconds=30000000) - and self.target_in_frame(context, rover_SE3, target_pos) - ) + context.node.get_logger().info("Time diff" + str(time_diff)) + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=30000000) and self.target_in_frame(context, rover_SE3, target_pos) def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): rover_to_model = target_pos - rover_SE3.translation() @@ -470,6 +452,7 @@ def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): rover_dot_model = np.dot(rover_to_model, rover_forward) angle_to_model = np.arccos(rover_dot_model) angle_to_model = math.copysign(angle_to_model, np.cross(rover_forward, rover_to_model)[2]) + context.node.get_logger().info("Angle to model" + str(angle_to_model)) return angle_to_model < self.STOP_ANGLE_THRESHOLD and angle_to_model > -1 * self.STOP_ANGLE_THRESHOLD def point_in_distance_threshold(self, context: Context, point): diff --git a/navigation/trajectory.py b/navigation/trajectory.py index f21351ed7..84cb8d255 100644 --- a/navigation/trajectory.py +++ b/navigation/trajectory.py @@ -28,16 +28,6 @@ def decerement_point(self) -> bool: self.cur_pt = max(0, self.cur_pt - 1) return self.cur_pt <= 0 - def add_end_point(self, position: np.ndarray) -> int: - """ - Adds point to end of trajectory, returns index of end point added - """ - if (len(self.coordinates)) == 0: - self.coordinates = np.atleast_2d(position) - else: - self.coordinates = np.vstack((self.coordinates, position)) - return len(self.coordinates) - 1 - def done(self) -> bool: return self.cur_pt >= len(self.coordinates) From b9a03e87184b11544f8d5130bb485d064a916a89 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Thu, 16 Apr 2026 18:54:07 -0400 Subject: [PATCH 17/31] Parameterized update time and increased it --- config/navigation.yaml | 1 + navigation/approach_target.py | 4 +++- navigation/nav.py | 1 + 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index 8f74f37b8..3b371af18 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -69,6 +69,7 @@ navigation: distance_threshold: 1.0 distance_look_threshold: 5.0 stop_angle_threshold: 0.2094 #pi/15 radians / 12 degrees + check_update_time: 0.03 #seconds update_delay: 3.0 single_tag: diff --git a/navigation/approach_target.py b/navigation/approach_target.py index c55729a12..1cbe19216 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -23,6 +23,7 @@ class ApproachTargetState(State): DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float STOP_ANGLE_THRESHOLD: float + CHECK_UPDATE_TIME: float time_begin: Time astar_traj: Trajectory target_traj: Trajectory @@ -54,6 +55,7 @@ def on_enter(self, context: Context) -> None: 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.STOP_ANGLE_THRESHOLD = context.node.get_parameter("search.stop_angle_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) @@ -442,7 +444,7 @@ def self_in_distance_threshold(self, context: Context, object_type: int) -> tupl return distance_to_target < self.DISTANCE_THRESHOLD, False else: context.node.get_logger().info("Time diff" + str(time_diff)) - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=30000000) and self.target_in_frame(context, rover_SE3, target_pos) + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10^9) and self.target_in_frame(context, rover_SE3, target_pos) def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): rover_to_model = target_pos - rover_SE3.translation() diff --git a/navigation/nav.py b/navigation/nav.py index 2f03401e5..3cf78811e 100755 --- a/navigation/nav.py +++ b/navigation/nav.py @@ -84,6 +84,7 @@ def __init__(self, ctx: Context) -> None: ("search.distance_threshold", Parameter.Type.DOUBLE), ("search.distance_look_threshold", Parameter.Type.DOUBLE), ("search.stop_angle_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), From 3f6599aaa84e723b6e34a02a060117ba268b3be8 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Thu, 16 Apr 2026 19:07:40 -0400 Subject: [PATCH 18/31] Fixed double error --- config/navigation.yaml | 2 +- navigation/nav.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index 3b371af18..deddc1b19 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -69,7 +69,7 @@ navigation: distance_threshold: 1.0 distance_look_threshold: 5.0 stop_angle_threshold: 0.2094 #pi/15 radians / 12 degrees - check_update_time: 0.03 #seconds + check_update_time: 0.06 #seconds update_delay: 3.0 single_tag: diff --git a/navigation/nav.py b/navigation/nav.py index 3cf78811e..3db2ce5bc 100755 --- a/navigation/nav.py +++ b/navigation/nav.py @@ -84,7 +84,7 @@ def __init__(self, ctx: Context) -> None: ("search.distance_threshold", Parameter.Type.DOUBLE), ("search.distance_look_threshold", Parameter.Type.DOUBLE), ("search.stop_angle_threshold", Parameter.Type.DOUBLE), - ("search.check_update_time", 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), From 7def3f2c8a08478fa1229861187828e98cc99319 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Thu, 16 Apr 2026 19:15:18 -0400 Subject: [PATCH 19/31] Fixed exponent error --- navigation/approach_target.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 1cbe19216..083d8cdbe 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -444,7 +444,7 @@ def self_in_distance_threshold(self, context: Context, object_type: int) -> tupl return distance_to_target < self.DISTANCE_THRESHOLD, False else: context.node.get_logger().info("Time diff" + str(time_diff)) - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10^9) and self.target_in_frame(context, rover_SE3, target_pos) + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10 ** 9) and self.target_in_frame(context, rover_SE3, target_pos) def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): rover_to_model = target_pos - rover_SE3.translation() From 084766cc7daeefb0083f244e45764836b32797b5 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Thu, 16 Apr 2026 20:12:40 -0400 Subject: [PATCH 20/31] Removed angle check --- config/navigation.yaml | 1 - navigation/approach_target.py | 15 +-------------- navigation/nav.py | 1 - 3 files changed, 1 insertion(+), 16 deletions(-) diff --git a/config/navigation.yaml b/config/navigation.yaml index deddc1b19..8d35c7950 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -68,7 +68,6 @@ navigation: angle_thresh: 0.0872665 #pi/36 radians / 5 degrees distance_threshold: 1.0 distance_look_threshold: 5.0 - stop_angle_threshold: 0.2094 #pi/15 radians / 12 degrees check_update_time: 0.06 #seconds update_delay: 3.0 diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 083d8cdbe..7755dc4da 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -22,7 +22,6 @@ class ApproachTargetState(State): WITHIN_DIST: bool DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float - STOP_ANGLE_THRESHOLD: float CHECK_UPDATE_TIME: float time_begin: Time astar_traj: Trajectory @@ -54,7 +53,6 @@ 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.STOP_ANGLE_THRESHOLD = context.node.get_parameter("search.stop_angle_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([])) @@ -444,18 +442,7 @@ def self_in_distance_threshold(self, context: Context, object_type: int) -> tupl return distance_to_target < self.DISTANCE_THRESHOLD, False else: context.node.get_logger().info("Time diff" + str(time_diff)) - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10 ** 9) and self.target_in_frame(context, rover_SE3, target_pos) - - def target_in_frame(self, context: Context, rover_SE3, target_pos: np.ndarray): - rover_to_model = target_pos - rover_SE3.translation() - rover_norm_model = np.linalg.norm(rover_to_model) - rover_to_model /= rover_norm_model - rover_forward = rover_SE3.rotation()[:, 0] - rover_dot_model = np.dot(rover_to_model, rover_forward) - angle_to_model = np.arccos(rover_dot_model) - angle_to_model = math.copysign(angle_to_model, np.cross(rover_forward, rover_to_model)[2]) - context.node.get_logger().info("Angle to model" + str(angle_to_model)) - return angle_to_model < self.STOP_ANGLE_THRESHOLD and angle_to_model > -1 * self.STOP_ANGLE_THRESHOLD + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, 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/nav.py b/navigation/nav.py index 3db2ce5bc..55fc01e66 100755 --- a/navigation/nav.py +++ b/navigation/nav.py @@ -83,7 +83,6 @@ def __init__(self, ctx: Context) -> None: ("search.angle_thresh", Parameter.Type.DOUBLE), ("search.distance_threshold", Parameter.Type.DOUBLE), ("search.distance_look_threshold", Parameter.Type.DOUBLE), - ("search.stop_angle_threshold", Parameter.Type.DOUBLE), ("search.check_update_time", Parameter.Type.DOUBLE), # Image Targets ("image_targets.increment_weight", Parameter.Type.INTEGER), From e6bf38d07a17f76e03afe2c13f17d7038f40f3a2 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Thu, 16 Apr 2026 20:21:39 -0400 Subject: [PATCH 21/31] removed logging --- navigation/approach_target.py | 1 - 1 file changed, 1 deletion(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 7755dc4da..99d5c18bb 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -441,7 +441,6 @@ def self_in_distance_threshold(self, context: Context, object_type: int) -> tupl if object_type in self.no_look_ahead_dict.values(): return distance_to_target < self.DISTANCE_THRESHOLD, False else: - context.node.get_logger().info("Time diff" + str(time_diff)) return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10 ** 9) def point_in_distance_threshold(self, context: Context, point): From c69a9f0eeed24ad1ca20ee4cad5723fd5af33819 Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 16 Apr 2026 20:32:05 -0400 Subject: [PATCH 22/31] Ready for merge 2 --- navigation/approach_target.py | 49 +++++++++++++++++++---------------- 1 file changed, 27 insertions(+), 22 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 99d5c18bb..13d64de1c 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -14,8 +14,6 @@ from navigation.coordinate_utils import is_high_cost_point, d_calc, segment_path, cartesian_to_ij - - class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool @@ -170,11 +168,11 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self if self.WITHIN_DIST: cmd_vel, arrived = self.spin_rover(context, self.target_position) - if(arrived): + if arrived: return costmap_search.CostmapSearchState() context.rover.send_drive_command(cmd_vel) - #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD - if(all(self.self_in_distance_threshold(context, self.object_type))): + # SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD + if all(self.self_in_distance_threshold(context, self.object_type)): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context, True) return self @@ -236,13 +234,13 @@ def on_loop_costmap_enabled(self, context: Context) -> State: # If we are within the distance threshold of the target we have finished self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.object_type) - if(self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST): + if self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST: context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - elif(self.object_type not in self.no_look_ahead_dict.values() and within_frame): + elif self.object_type not in self.no_look_ahead_dict.values() and within_frame: context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - elif(self.WITHIN_DIST): + elif self.WITHIN_DIST: context.node.get_logger().info("Object within distance but not within frame") arrived = False @@ -288,7 +286,8 @@ def on_loop_costmap_enabled(self, context: Context) -> State: context.rover.send_drive_command(cmd_vel) return self - #Fix disabled state soon + + # Fix disabled state soon def on_loop_costmap_disabled(self, context: Context) -> State: from .long_range import LongRangeState @@ -297,14 +296,14 @@ def on_loop_costmap_disabled(self, context: Context) -> State: if self.target_position is None: return self - - if(self.WITHIN_DIST): + + if self.WITHIN_DIST: cmd_vel, arrived = self.spin_rover(context, self.target_position) - if(arrived): + if arrived: return costmap_search.CostmapSearchState() context.rover.send_drive_command(cmd_vel) - #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD - if(all(self.self_in_distance_threshold(context, self.object_type))): + # SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD + if all(self.self_in_distance_threshold(context, self.object_type)): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context, True) return self @@ -318,13 +317,13 @@ def on_loop_costmap_disabled(self, context: Context) -> State: context.node.get_parameter("waypoint.drive_forward_threshold").value, ) self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.object_type) - if(self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST): + if self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST: context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - elif(self.object_type not in self.no_look_ahead_dict.values() and within_frame): + elif self.object_type not in self.no_look_ahead_dict.values() and within_frame: context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - elif(self.WITHIN_DIST): + elif self.WITHIN_DIST: context.node.get_logger().info("Object within distance but not within frame") if arrived: if isinstance(self, LongRangeState): @@ -420,12 +419,16 @@ def display_markers(self, context: Context): context.publish_path_marker( points=np.array([self.target_position]), color=[1.0, 1.0, 0.0], ns=str(type(self)) ) - def spin_rover(self, context: Context, target_pos: np.ndarray): + + def spin_rover(self, context: Context, target_pos: np.ndarray | None): + if target_pos is None: + return Twist(), True cmd_vel, arrived = context.drive.get_drive_command( - target_pos, context.rover.get_pose_in_map(), - self.DISTANCE_THRESHOLD, 1e-8) + target_pos, context.rover.get_pose_in_map(), self.DISTANCE_THRESHOLD, 1e-8 + ) return cmd_vel, arrived - def self_in_distance_threshold(self, context: Context, object_type: int) -> tuple[bool | bool]: + + def self_in_distance_threshold(self, context: Context, object_type: int) -> tuple[bool, bool]: rover_SE3 = context.rover.get_pose_in_map() if rover_SE3 is None: return False, False @@ -441,7 +444,9 @@ def self_in_distance_threshold(self, context: Context, object_type: int) -> tupl if object_type in self.no_look_ahead_dict.values(): return distance_to_target < self.DISTANCE_THRESHOLD, False else: - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10 ** 9) + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration( + nanoseconds=self.CHECK_UPDATE_TIME * 10**9 + ) def point_in_distance_threshold(self, context: Context, point): if point is None: From 897f345d8e6d445b52283536d74ac97177c46b25 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Tue, 21 Apr 2026 18:16:45 -0400 Subject: [PATCH 23/31] Changed object type to use is_object bool --- navigation/approach_target.py | 26 ++++++++++++-------------- navigation/context.py | 2 +- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 99d5c18bb..35800be6d 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -31,8 +31,7 @@ class ApproachTargetState(State): target_position: np.ndarray | None marker_timer: Timer update_timer: Timer - object_type: int - no_look_ahead_dict: dict + looking_for_object: bool def on_enter(self, context: Context) -> None: from .long_range import LongRangeState @@ -62,8 +61,7 @@ def on_enter(self, context: Context) -> None: self.WITHIN_DIST = False self.time_last_updated = context.node.get_clock().now() self.time_begin = context.node.get_clock().now() - self.object_type = current_waypoint.type.val - self.no_look_ahead_dict = {"NO_SEARCH": 0, "POST": 1} + 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) @@ -174,7 +172,7 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return costmap_search.CostmapSearchState() context.rover.send_drive_command(cmd_vel) #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD - if(all(self.self_in_distance_threshold(context, self.object_type))): + if(all(self.self_in_distance_threshold(context, self.looking_for_object))): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context, True) return self @@ -235,11 +233,11 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self # If we are within the distance threshold of the target we have finished - self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.object_type) - if(self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST): + self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.looking_for_object) + if(self.looking_for_object and self.WITHIN_DIST): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - elif(self.object_type not in self.no_look_ahead_dict.values() and within_frame): + elif(self.looking_for_object and within_frame): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) elif(self.WITHIN_DIST): @@ -304,7 +302,7 @@ def on_loop_costmap_disabled(self, context: Context) -> State: return costmap_search.CostmapSearchState() context.rover.send_drive_command(cmd_vel) #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD - if(all(self.self_in_distance_threshold(context, self.object_type))): + if(all(self.self_in_distance_threshold(context, self.looking_for_object))): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context, True) return self @@ -317,11 +315,11 @@ 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, ) - self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.object_type) - if(self.object_type in self.no_look_ahead_dict.values() and self.WITHIN_DIST): + self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.looking_for_object) + if(self.looking_for_object and self.WITHIN_DIST): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) - elif(self.object_type not in self.no_look_ahead_dict.values() and within_frame): + elif(self.looking_for_object and within_frame): context.node.get_logger().info("Exited through distance threshold") return self.next_state(context=context, is_finished=True) elif(self.WITHIN_DIST): @@ -425,7 +423,7 @@ def spin_rover(self, context: Context, target_pos: np.ndarray): target_pos, context.rover.get_pose_in_map(), self.DISTANCE_THRESHOLD, 1e-8) return cmd_vel, arrived - def self_in_distance_threshold(self, context: Context, object_type: int) -> tuple[bool | bool]: + def self_in_distance_threshold(self, context: Context, is_object: bool) -> tuple[bool | bool]: rover_SE3 = context.rover.get_pose_in_map() if rover_SE3 is None: return False, False @@ -438,7 +436,7 @@ def self_in_distance_threshold(self, context: Context, object_type: int) -> tupl return False, False rover_translation = rover_SE3.translation()[0:2] distance_to_target = d_calc(rover_translation, tuple(target_pos)) - if object_type in self.no_look_ahead_dict.values(): + if is_object: return distance_to_target < self.DISTANCE_THRESHOLD, False else: return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration(nanoseconds=self.CHECK_UPDATE_TIME * 10 ** 9) diff --git a/navigation/context.py b/navigation/context.py index d52fbc65f..42648bc44 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -280,7 +280,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 { From bd73d79d6eaf4648e0b2b7dcd24a40974421e457 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Tue, 21 Apr 2026 19:26:13 -0400 Subject: [PATCH 24/31] Fixed changes, including removing WITHIN_DIST, splitting in_distance_threshold to two functions, and moving spinning to drive.py --- navigation/approach_target.py | 90 ++++++++++++----------------------- navigation/drive.py | 7 +++ 2 files changed, 38 insertions(+), 59 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index d1fc52659..18224686a 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -17,7 +17,6 @@ class ApproachTargetState(State): UPDATE_DELAY: float USE_COSTMAP: bool - WITHIN_DIST: bool DISTANCE_THRESHOLD: float LOOK_DISTANCE_THRESHOLD: float CHECK_UPDATE_TIME: float @@ -55,8 +54,6 @@ def on_enter(self, context: Context) -> None: self.target_traj = Trajectory(np.array([])) self.astar = AStar(context=context) self.target_position = None - self.fixed_position = None - self.WITHIN_DIST = False 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() @@ -164,16 +161,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 self.WITHIN_DIST: - cmd_vel, arrived = self.spin_rover(context, self.target_position) - if arrived: - return costmap_search.CostmapSearchState() - context.rover.send_drive_command(cmd_vel) - #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD - if(all(self.self_in_distance_threshold(context, self.looking_for_object))): - context.node.get_logger().info("Exited through distance threshold") - return self.next_state(context, True) - 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") @@ -231,15 +218,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self # If we are within the distance threshold of the target we have finished - self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.looking_for_object) - if(self.looking_for_object and self.WITHIN_DIST): - context.node.get_logger().info("Exited through distance threshold") - return self.next_state(context=context, is_finished=True) - elif(not self.looking_for_object and within_frame): - context.node.get_logger().info("Exited through distance threshold") - return self.next_state(context=context, is_finished=True) - elif self.WITHIN_DIST: - context.node.get_logger().info("Object within distance but not within frame") arrived = False cmd_vel = Twist() @@ -286,7 +264,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State: return self - # Fix disabled state soon def on_loop_costmap_disabled(self, context: Context) -> State: from .long_range import LongRangeState @@ -296,16 +273,6 @@ def on_loop_costmap_disabled(self, context: Context) -> State: if self.target_position is None: return self - if self.WITHIN_DIST: - cmd_vel, arrived = self.spin_rover(context, self.target_position) - if arrived: - return costmap_search.CostmapSearchState() - context.rover.send_drive_command(cmd_vel) - #SPIN ROVER AND CHECK IF IT IS IN DISTANCE THRESHOLD - if(all(self.self_in_distance_threshold(context, self.looking_for_object))): - context.node.get_logger().info("Exited through distance threshold") - return self.next_state(context, True) - return self arrived = False cmd_vel = Twist() @@ -315,15 +282,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, ) - self.WITHIN_DIST, within_frame = self.self_in_distance_threshold(context, self.looking_for_object) - if(self.looking_for_object and self.WITHIN_DIST): - context.node.get_logger().info("Exited through distance threshold") - return self.next_state(context=context, is_finished=True) - elif(not self.looking_for_object and within_frame): - context.node.get_logger().info("Exited through distance threshold") - return self.next_state(context=context, is_finished=True) - elif self.WITHIN_DIST: - context.node.get_logger().info("Object within distance but not within frame") if arrived: if isinstance(self, LongRangeState): self.target_position = self.get_target_position(context) @@ -395,6 +353,23 @@ 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") + cmd_vel, arrived = context.drive.spin_rover(context.rover.get_pose_in_map(), self.target_position) + #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: @@ -419,32 +394,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 spin_rover(self, context: Context, target_pos: np.ndarray | None): - if target_pos is None: - return Twist(), True - cmd_vel, arrived = context.drive.get_drive_command( - target_pos, context.rover.get_pose_in_map(), self.DISTANCE_THRESHOLD, 1e-8 - ) - return cmd_vel, arrived - def self_in_distance_threshold(self, context: Context, is_object: bool) -> tuple[bool | bool]: + + 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, False + return False target_pos = context.env.current_target_pos() if target_pos is None: - return False, False - time_diff = context.env.current_time_diff() - if time_diff is None: - return False, False + return False rover_translation = rover_SE3.translation()[0:2] distance_to_target = d_calc(rover_translation, tuple(target_pos)) if is_object: - return distance_to_target < self.DISTANCE_THRESHOLD, False + return distance_to_target < self.LOOK_DISTANCE_THRESHOLD else: - return distance_to_target < self.LOOK_DISTANCE_THRESHOLD, time_diff < Duration( - nanoseconds=self.CHECK_UPDATE_TIME * 10**9 - ) + return distance_to_target < self.DISTANCE_THRESHOLD + + def looking_at_object(self, context: Context) -> bool: + 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/drive.py b/navigation/drive.py index ae7b20b4c..bbe6fff2a 100644 --- a/navigation/drive.py +++ b/navigation/drive.py @@ -215,6 +215,13 @@ def get_drive_command( target_pos, rover_pose, ) + def spin_rover(self, rover_pose: SE3, target_pos: np.ndarray | None) -> tuple[Twist, bool]: + if target_pos is None: + return Twist(), True + cmd_vel, arrived = self.get_drive_command( + target_pos, rover_pose, self.DISTANCE_THRESHOLD, 1e-8 + ) + return cmd_vel, arrived def get_default_drive_command( self: DriveController, From a4662abba680ae0d2208acc8bf9aced670b35f26 Mon Sep 17 00:00:00 2001 From: Beastvr00k Date: Tue, 21 Apr 2026 19:33:20 -0400 Subject: [PATCH 25/31] Got rid of try catch for testing --- state_machine/state_machine.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/state_machine/state_machine.py b/state_machine/state_machine.py index 58e4e090d..4b6df7518 100644 --- a/state_machine/state_machine.py +++ b/state_machine/state_machine.py @@ -59,14 +59,14 @@ def update(self): raise Exception(f"Invalid transition from {current_state} to {next_state}") if type(next_state) is not type(current_state): # TODO: Make sure no exceptions - try: - self.logger.debug(f"{self.name} state machine, transitioning to {str(next_state)}") - current_state.on_exit(self.context) - self.transition_log.append(TransitionRecord(time.time(), str(current_state), str(next_state))) - self.current_state = next_state - self.current_state.on_enter(self.context) - except Exception as e: - self.logger.warn(f"Error in {str(current_state)}: {e}") + #try: + self.logger.debug(f"{self.name} state machine, transitioning to {str(next_state)}") + current_state.on_exit(self.context) + self.transition_log.append(TransitionRecord(time.time(), str(current_state), str(next_state))) + self.current_state = next_state + self.current_state.on_enter(self.context) + #except Exception as e: + #self.logger.warn(f"Error in {str(current_state)}: {e}") def add_transition(self, state_from: State, state_to: State) -> None: self.state_transitions[type(state_from)].add(type(state_to)) From 87e282686ba1df759e63b40273659e85d3ac6c1c Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 23 Apr 2026 21:56:28 -0400 Subject: [PATCH 26/31] Fixed error and tested --- navigation/approach_target.py | 22 ++++++++++------------ navigation/drive.py | 7 +++---- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 18224686a..aa57fdc3e 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -273,7 +273,6 @@ def on_loop_costmap_disabled(self, context: Context) -> State: if self.target_position is None: return self - arrived = False cmd_vel = Twist() cmd_vel, arrived = context.drive.get_drive_command( @@ -354,18 +353,20 @@ def on_loop(self, context: Context) -> State: 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)): + # 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 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") - cmd_vel, arrived = context.drive.spin_rover(context.rover.get_pose_in_map(), self.target_position) - #If we have finished spinning and haven't moved onto a different state, we want to enter the costmap search state - if(arrived): + 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 we want to have the spin drive command applied to the rover else: context.rover.send_drive_command(cmd_vel) return self @@ -394,7 +395,6 @@ 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, is_object: bool) -> bool: rover_SE3 = context.rover.get_pose_in_map() if rover_SE3 is None: @@ -414,9 +414,7 @@ def looking_at_object(self, context: Context) -> bool: 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) - + 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/drive.py b/navigation/drive.py index bbe6fff2a..b4209a8b3 100644 --- a/navigation/drive.py +++ b/navigation/drive.py @@ -215,12 +215,11 @@ def get_drive_command( target_pos, rover_pose, ) - def spin_rover(self, rover_pose: SE3, target_pos: np.ndarray | None) -> tuple[Twist, bool]: + + 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 - cmd_vel, arrived = self.get_drive_command( - target_pos, rover_pose, self.DISTANCE_THRESHOLD, 1e-8 - ) + cmd_vel, arrived = self.get_drive_command(target_pos, rover_pose, distance_thresh, 1e-8) return cmd_vel, arrived def get_default_drive_command( From 1c2774a7ae06d4b55ae021840633dff0f7400b39 Mon Sep 17 00:00:00 2001 From: Vishal Date: Thu, 23 Apr 2026 21:58:03 -0400 Subject: [PATCH 27/31] Undoing state machine changes --- state_machine/state_machine.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/state_machine/state_machine.py b/state_machine/state_machine.py index 4b6df7518..58e4e090d 100644 --- a/state_machine/state_machine.py +++ b/state_machine/state_machine.py @@ -59,14 +59,14 @@ def update(self): raise Exception(f"Invalid transition from {current_state} to {next_state}") if type(next_state) is not type(current_state): # TODO: Make sure no exceptions - #try: - self.logger.debug(f"{self.name} state machine, transitioning to {str(next_state)}") - current_state.on_exit(self.context) - self.transition_log.append(TransitionRecord(time.time(), str(current_state), str(next_state))) - self.current_state = next_state - self.current_state.on_enter(self.context) - #except Exception as e: - #self.logger.warn(f"Error in {str(current_state)}: {e}") + try: + self.logger.debug(f"{self.name} state machine, transitioning to {str(next_state)}") + current_state.on_exit(self.context) + self.transition_log.append(TransitionRecord(time.time(), str(current_state), str(next_state))) + self.current_state = next_state + self.current_state.on_enter(self.context) + except Exception as e: + self.logger.warn(f"Error in {str(current_state)}: {e}") def add_transition(self, state_from: State, state_to: State) -> None: self.state_transitions[type(state_from)].add(type(state_to)) From c4349280ef50ac0c9f9dd56d614e6c840fe4cdf3 Mon Sep 17 00:00:00 2001 From: Vishal Date: Fri, 24 Apr 2026 01:32:24 -0400 Subject: [PATCH 28/31] Added comments, added rock pick --- navigation/approach_target.py | 4 ++++ navigation/context.py | 8 ++++++-- navigation/drive.py | 1 + 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index aa57fdc3e..023d8114e 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -360,6 +360,7 @@ def on_loop(self, context: Context) -> State: # 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 ) @@ -404,13 +405,16 @@ def self_in_distance_threshold(self, context: Context, is_object: bool) -> bool: 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)) + #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 diff --git a/navigation/context.py b/navigation/context.py index 2306a80e5..8d45b5c37 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -110,8 +110,9 @@ 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: - waste, t = SE3.from_tf_tree_with_time(self.ctx.tf_buffer, frame, self.ctx.world_frame) + _, t = SE3.from_tf_tree_with_time(self.ctx.tf_buffer, frame, self.ctx.world_frame) except ( tf2_ros.LookupException, tf2_ros.ConnectivityException, @@ -120,6 +121,7 @@ def get_time_diff(self, frame: str) -> None | Duration: 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 @@ -140,7 +142,7 @@ def current_target_pos(self) -> np.ndarray | 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}") @@ -148,6 +150,8 @@ def current_time_diff(self): 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_target_position("pick") case _: return None diff --git a/navigation/drive.py b/navigation/drive.py index b4209a8b3..b2be7f4cb 100644 --- a/navigation/drive.py +++ b/navigation/drive.py @@ -219,6 +219,7 @@ def get_drive_command( 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 From 3cdc14cac14954526027025659d5e6f733e6ca99 Mon Sep 17 00:00:00 2001 From: Vishal Date: Fri, 24 Apr 2026 01:36:26 -0400 Subject: [PATCH 29/31] Style checks --- navigation/approach_target.py | 12 ++++-------- navigation/context.py | 6 +++--- navigation/drive.py | 2 +- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 023d8114e..6b1cad4af 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -1,6 +1,4 @@ import numpy as np -import math -from typing import Any, Literal from navigation.trajectory import Trajectory from navigation.astar import AStar, NoPath, OutOfBounds from . import costmap_search, stuck_recovery, waypoint, backup, state @@ -217,8 +215,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State: context.node.get_logger().info("Found low-cost point") return self - # If we are within the distance threshold of the target we have finished - arrived = False cmd_vel = Twist() if not self.astar_traj.done(): @@ -360,7 +356,7 @@ def on_loop(self, context: Context) -> State: # 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 + # 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 ) @@ -405,16 +401,16 @@ def self_in_distance_threshold(self, context: Context, is_object: bool) -> bool: if target_pos is None: return False rover_translation = rover_SE3.translation()[0:2] - #Calculate the distance from the rover to the target + # Calculate the distance from the rover to the target distance_to_target = d_calc(rover_translation, tuple(target_pos)) - #If the target isn't an object, use a smaller distance + # 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 + # 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 diff --git a/navigation/context.py b/navigation/context.py index 8d45b5c37..81ac07a88 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -110,7 +110,7 @@ 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 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 ( @@ -121,7 +121,7 @@ def get_time_diff(self, frame: str) -> None | Duration: return None now = self.ctx.node.get_clock().now() - #Calculate difference between current time and TF tree time + # Calculate difference between current time and TF tree time time = Time.from_msg(t) return now - time @@ -142,7 +142,7 @@ def current_target_pos(self) -> np.ndarray | None: def current_time_diff(self): assert self.ctx.course is not None - #Return the time difference with an associated target frame + # 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}") diff --git a/navigation/drive.py b/navigation/drive.py index b2be7f4cb..f65ec07c3 100644 --- a/navigation/drive.py +++ b/navigation/drive.py @@ -219,7 +219,7 @@ def get_drive_command( 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 + # 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 From f84bbb9ea653b818c17476bd201c97af2703a828 Mon Sep 17 00:00:00 2001 From: Vishal Date: Fri, 24 Apr 2026 01:39:29 -0400 Subject: [PATCH 30/31] fixed small changes --- navigation/approach_target.py | 1 - 1 file changed, 1 deletion(-) diff --git a/navigation/approach_target.py b/navigation/approach_target.py index 6b1cad4af..a84723b61 100644 --- a/navigation/approach_target.py +++ b/navigation/approach_target.py @@ -252,7 +252,6 @@ def on_loop_costmap_enabled(self, context: Context) -> State: # Fully dilated and still failed, go to next state context.node.get_logger().info("Exited without distance threshold") return self.next_state(context, is_finished=True) - # return self.next_state(context=context, is_finished=True) return self else: From e5525e7825410a9ae4f65dd90741c74af594dca2 Mon Sep 17 00:00:00 2001 From: Vishal Date: Fri, 24 Apr 2026 01:40:48 -0400 Subject: [PATCH 31/31] Fixed another small error in context --- navigation/context.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/navigation/context.py b/navigation/context.py index 81ac07a88..9439c402e 100644 --- a/navigation/context.py +++ b/navigation/context.py @@ -151,7 +151,7 @@ def current_time_diff(self): 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_target_position("pick") + return self.get_time_diff("pick") case _: return None