Merge pull request #155 from MissouriMRDT/feature/obstacle_avoidance

Updated Competition Merge (dev)

algorithms/ar_tag.py

@@ -131,8 +131,9 @@ def add_tag(self, tag_id, corners) -> None:
                     if tag.id == tag_id:
                         # Don't go over detection limit.
                         if tag.times_detected < core.constants.ARUCO_MAX_FRAMES_DETECTED:
-                            # Increment number of times tag has been seen.
-                            tag.times_detected += 1
+                            if (core.states.state_machine.get_state_str() != "ApproachingGate"):
+                                # Increment number of times tag has been seen.
+                                tag.times_detected += 1
 
                         # Refresh tag distance and angle.
                         tag.refresh((cX, cY))

algorithms/gps_navigate.py

@@ -67,7 +67,7 @@ def calculate_move(goal, location, start, speed=0.6*MAX_DRIVE_POWER):
     logger.debug(f"Target distance: {target_distance}")
 
     if target_distance < 0.01:
-        speed = 0.4*MAX_DRIVE_POWER
+        speed = 0.4 * speed
 
     goal_heading = target_heading
     logger.debug(f"Current heading: {interfaces.nav_board.heading()}, Goal: {goal_heading}")

algorithms/marker_search.py

@@ -36,9 +36,15 @@ def calculate_next_coordinate(start, former_goal):
     # Formula for archimedes spiral is r = aθ, calculate current theta using a known a
     # (search distance) and a known r (distance to center, or starting point)
     theta = r / (core.SEARCH_DISTANCE / 1000)
-
-    # Add delta theta to theta
-    theta += core.DELTA_THETA
+    # If we are toggled to turn left then make the delta theta negative.
+    if core.constants.SEARCH_LEFT:
+        # Invert theta.
+        theta *= -1
+        # Subtract delta theta from theta.
+        theta -= core.constants.SEARCH_DELTA_THETA
+    else:
+        # Add delta theta to theta
+        theta += core.constants.SEARCH_DELTA_THETA
 
     # Now that we have a new θ, calculate the new radius given a and θ
     r = (core.SEARCH_DISTANCE / 1000) * theta

algorithms/small_movements.py

@@ -24,7 +24,7 @@ def backup(start_latitude, start_longitude, target_distance, speed=-200):
     speed = -abs(speed)
 
     # Get total distance traveled so far.
-    current_latitude, current_longitude = interfaces.nav_board.location()
+    current_latitude, current_longitude = interfaces.nav_board.location(force_absolute=True)
     bearing, distance_traveled = algorithms.geomath.haversine(
         start_latitude, start_longitude, current_latitude, current_longitude
     )

autonomy.py

@@ -79,9 +79,12 @@ async def autonomy_state_loop():
         )
 
         # Print debug
-        print(interfaces.nav_board.location())
-        print(interfaces.nav_board.heading())
-        print(interfaces.nav_board._heading)
+        # relpos = interfaces.nav_board.location()
+        # abspos = interfaces.nav_board.location(force_absolute=True)
+        # print("RELPOS:", utm.from_latlon(relpos[0], relpos[1])[:2])
+        # print("ABSPOS:", utm.from_latlon(abspos[0], abspos[1])[:2])
+        # print("RELHEAD:", interfaces.nav_board.heading())
+        # print("ABSHEAD:", interfaces.nav_board._heading)
 
         # Core state machine runs every X ms, to prevent unnecessarily fast computation.
         # Sensor data is processed separately, as that is the bulk of processing time

core/constants.py

@@ -28,7 +28,7 @@
 SCALING_FACTOR = 10.0  # pixel-meters
 WAYPOINT_DISTANCE_THRESHOLD = 0.5  # maximum threshold in meters between rover and waypoint
 BEARING_FLIP_THRESHOLD = 30.0  # 180 +/- this many degrees counts as a flip in bearing
-MAX_DRIVE_POWER = 450  # -1000 to 1000, normally 250 dropped lower for early testing to be safe
+MAX_DRIVE_POWER = 425  # -1000 to 1000, normally 250 dropped lower for early testing to be safe
 MIN_DRIVE_POWER = -250  # -1000 to 1000, normally 50
 GATE_POINT_DISTANCES = 3.0
 NAVIGATION_PATH_EXPIRATION_SECONDS = 5  # The time in seconds before a new path is force generated.
@@ -38,6 +38,7 @@
 NAVIGATION_START_BACKUP_DISTANCE = 2  # Time to backup if tag is in front of rover when entering nav state.
 NAVIGATION_BACKUP_SPEED = -250  # the speed to backup with.
 NAVIGATION_BACKUP_TAG_DISTANCE_THRESH = 3  # Min distance tag can be from rover to trigger backup.
+NAVIGATION_ALWAYS_REVERSE_OUT_OF_IDLE = True # If true rover will always enter reverse state from idle. Then go to nav.
 
 # Approaching Gate Parameters.
 GATE_WAYPOINT_THRESH = 0.3  # The minimum distance from end waypoint before we consider ourselves there.
@@ -58,10 +59,11 @@
 
 # Search Pattern Parameters
 SEARCH_DISTANCE = 4  # meters
-SEARCH_DRIVE_POWER = 250
+SEARCH_DRIVE_POWER = 400
 SEARCH_PATTERN_MAX_ERROR_FROM_PATH = 5  # The max distance the rover diverge off path before regen.
 SEARCH_OBSTACLE_QUEUE_LENGTH = 10  # The max number of objects to store at once.
-DELTA_THETA = math.pi / 4
+SEARCH_DELTA_THETA = math.pi / 4 # Pattern/shape/vertices of spiral.
+SEARCH_LEFT = False # Spiral turns left or right.
 
 # Obstacle Detection Parameters.
 DETECTION_MODEL_CONF = 0.4
@@ -77,9 +79,12 @@
 AVOIDANCE_OBSTACLE_QUEUE_LENGTH = 10  # The number of obstacles to store at a time.
 AVOIDANCE_MAX_SPEED_MPS = 0.6  # The max speed in meters per second to drive the rover. MUST MAKE SURE THIS IS ATTAINABLE WITH DRIVE SPEED POWER.
 
+# Stuck State Parameters.
+STUCK_STILL_TIME = 3.0 # The number of seconds to sit still in stuck state.
+
 # Vision Parameters
-ARUCO_FRAMES_DETECTED = 3  # ArUco Detection Occurrences
-ARUCO_MAX_FRAMES_DETECTED = 10  # Max frame counter for each tag.
+ARUCO_FRAMES_DETECTED = 2  # ArUco Detection Occurrences
+ARUCO_MAX_FRAMES_DETECTED = 15  # Max frame counter for each tag.
 ARUCO_MARKER_BORDER_BITS = 1
 ARUCO_ERROR_CORRECTION_RATE = 1
 ARUCO_ENABLE_DISTANCE = 25  # The minimum distance from the goal waypoint before aruco detection os considered valid.

core/states/approaching_gate.py

@@ -7,6 +7,7 @@
 #
 
 import core
+import copy
 from core import constants
 import interfaces
 from algorithms import stanley_controller, heading_hold
@@ -158,15 +159,15 @@ async def run(self) -> RoverState:
             # The update_obstacles method automatically converts the angles and distances to gps, so pull out gps coords.
             self.gate_coords = self.astar.get_obstacle_coords()
             # Convert gate gps coords to utm xs and ys.
-            gate_xs = [utm.from_latlon(t[0], t[1])[0] for t in self.gate_coords]
-            gate_ys = [utm.from_latlon(t[0], t[1])[1] for t in self.gate_coords]
+            gate_xs = [utm.from_latlon(t[0], t[1])[0] for t in copy.deepcopy(self.gate_coords)]
+            gate_ys = [utm.from_latlon(t[0], t[1])[1] for t in copy.deepcopy(self.gate_coords)]
 
             ######################################################################################################################
             # Calculate the intersection line and perp line to the obstacles to make a 'trench' for the rover to drive through.
             # Check https://www.desmos.com/calculator/xcbgsmlk6x for an interactive graph.
             ######################################################################################################################
             # Catch bad gate coords.
-            if len(gate_xs) <= 2:
+            if len(gate_xs) <= 2 and len(gate_ys) <= 2:
                 # Create list to store UTM coords.
                 self.gate1_obstacles.clear()
                 self.gate2_obstacles.clear()

core/states/idle.py

@@ -12,6 +12,8 @@
 import interfaces
 import algorithms
 from core.states import RoverState
+import utm
+import matplotlib.pyplot as plt
 
 
 class Idle(RoverState):
@@ -21,10 +23,24 @@ class Idle(RoverState):
     from base station that configure the next leg’s settings and confirm them.
     """
 
-    def __init__(self):
+    def start(self):
+        """
+        Schedule Idle
+        """
         self.logger = logging.getLogger(__name__)
         self.idle_time = time.time()
         self.realigned = False
+        self.rover_xs = []
+        self.rover_ys = []
+        self.max_draw_length = 100
+
+    def exit(self):
+        """
+        Cancel all state specific coroutines
+        """
+        # Clear rover path.
+        self.rover_xs.clear()
+        self.rover_ys.clear()
 
     def on_event(self, event) -> RoverState:
         """
@@ -51,8 +67,13 @@ def on_event(self, event) -> RoverState:
                 # Move to reversing state.
                 state = core.states.Reversing()
             else:
-                # Change states.
-                state = core.states.Navigating()
+                # Check reverse always toggle.
+                if core.constants.NAVIGATION_ALWAYS_REVERSE_OUT_OF_IDLE:
+                    # Change states.
+                    state = core.states.Reversing()
+                else:
+                    # Change states.
+                    state = core.states.Navigating()
 
         elif event == core.AutonomyEvents.ABORT:
             state = self
@@ -96,5 +117,33 @@ async def run(self):
             else:
                 # Reset toggle.
                 self.realigned = False
+                pass
+
+
+
+        # Store rover position path.
+        current = interfaces.nav_board.location()
+        utm_current = utm.from_latlon(current[0], current[1])
+        self.rover_xs.append(utm_current[0])
+        self.rover_ys.append(utm_current[1])
+        # Write path 1 second before it expires.
+        if int(time.time()) % 2 == 0:
+            plt.cla()
+            # Plot path, current location, and goal.
+            plt.gca().set_aspect("equal", adjustable="box")
+            plt.plot(self.rover_xs, self.rover_ys, "-b", label="trajectory")
+            # Plot rover.
+            plt.plot(utm_current[0], utm_current[1], "2", label="rover")
+            plt.axis("equal")
+            plt.grid(True)
+            plt.title(f"IDLE - Heading: {int(interfaces.nav_board.heading())}")
+            plt.savefig("logs/!rover_path.png")
+
+            # Check length of the rover path.
 
+            if len(self.rover_xs) > self.max_draw_length:
+                # Cutoff old points.
+                self.rover_xs = self.rover_xs[::-1][:self.max_draw_length][::-1]
+                self.rover_ys = self.rover_ys[::-1][:self.max_draw_length][::-1]
+
         return self

core/states/navigating.py

@@ -220,6 +220,9 @@ async def run(self) -> RoverState:
                     interfaces.multimedia_board.send_lighting_state(core.OperationState.REACHED_MARKER)
                     return self.on_event(core.AutonomyEvents.REACHED_MARKER)
                 else:
+                    # Set gps goal to our current position.
+                    core.waypoint_handler.set_goal(current)
+                    # Move to search pattern state.
                     return self.on_event(core.AutonomyEvents.REACHED_GPS_COORDINATE)
         else:
             # Force path to expire and regenerate.

core/states/reversing.py

@@ -25,7 +25,7 @@ def start(self):
         # Print log.
         self.logger.warning("BACKING UP! AR Tag detected in front of rover.")
         # Store current location when state is entered.
-        self.start_position = interfaces.nav_board.location()
+        self.start_position = interfaces.nav_board.location(force_absolute=True)
 
     def on_event(self, event) -> RoverState:
         """

core/states/search_pattern.py

@@ -87,8 +87,8 @@ async def run(self) -> RoverState:
         :return: RoverState
         """
         # Get current position and next desired waypoint position.
-        current = interfaces.nav_board.location()
-        gps_data = core.waypoint_handler.gps_data
+        current = interfaces.nav_board.location(force_absolute=True)
+        gps_data = core.waypoint_handler.get_waypoint()
 
         """
         STATE TRANSITION AND WAYPOINT LOGIC.
@@ -147,7 +147,7 @@ async def run(self) -> RoverState:
             return self.on_event(core.AutonomyEvents.MARKER_SEEN)
 
         if algorithms.gps_navigate.get_approach_status(goal, current, start) != core.ApproachState.APPROACHING:
-            interfaces.drive_board.stop()
+            # interfaces.drive_board.stop()
 
             # Sleep for a little bit before we move to the next point, allows for AR Tag to be picked up
             await asyncio.sleep(core.EVENT_LOOP_DELAY)
@@ -157,7 +157,7 @@ async def run(self) -> RoverState:
             core.waypoint_handler.set_goal(goal)
 
         # Calculate drive power.
-        left, right = algorithms.gps_navigate.calculate_move(goal, current, start, core.MAX_DRIVE_POWER)
+        left, right = algorithms.gps_navigate.calculate_move(goal, current, start, core.constants.SEARCH_DRIVE_POWER)
         # Send drive.
         interfaces.drive_board.send_drive(left, right)
         # Send drive.

core/states/state_machine.py

@@ -55,7 +55,7 @@ async def run(self):
         elif self.disable_flag is True:
             self.state = self.state.on_event(core.AutonomyEvents.ABORT)
             # Update the state display on lighting to Teleop
-            interfaces.multimedia_board.send_lighting_state(core.OperationState.TELEOP)
+            # interfaces.multimedia_board.send_lighting_state(core.OperationState.TELEOP)
             self.disable_flag = False
 
         # Run the current state

core/states/stuck.py

@@ -28,7 +28,9 @@ def start(self):
         interfaces.drive_board.stop()
         # Set random seed.
         random.seed(time.time())
-        self.rand_num = random.randint(1, 10)
+        self.rand_num = random.randint(1, 9)
+        # Get stuck state start timer.
+        self.start_timer = time.time()
 
     def on_event(self, event) -> RoverState:
         """
@@ -41,7 +43,7 @@ def on_event(self, event) -> RoverState:
 
         if event == core.AutonomyEvents.START:
             # Change states to Idle
-            state = core.states.Idle()
+            state = core.states.Reversing()
 
         elif event == core.AutonomyEvents.ABORT:
             # Change states to Idle
@@ -99,10 +101,10 @@ async def run(self):
             self.logger.warning(
                 "Dude, the Rover's like, 'Whoa, man, I'm totally stuck, bro.' But don't worry, there's a chill solution! Check it out, we got two gnarly buttons: 'START AUTONOMY' and 'STOP AUTONOMY.' No matter which one you hit, this Rover's just gonna kick back and chillax in the idle state, dude. So, like, choose your own adventure, man. Keep the autonomy flowin' or bring it back to chill mode. Either way, this Rover's just cruisin' and enjoyin' the cosmic vibes, man."
             )
-        elif self.rand_num == 10:
-            self.logger.warning(
-                "🤖🚀 Rover: '🆘 I'm 🚫🚶 stuck. ⏯️' 👉 Press ▶️ to activate 🤖🔀 or ❌ to deactivate 🤖🔀 and return to 😴 state. 🤖🚀 Rover: 'Either way, I'm 😎 chillin'!'"
-            )
+
+        # Check if we've been in stuck for more than 5 seconds.
+        if (time.time() - self.start_timer) > core.constants.STUCK_STILL_TIME:
+            return self.on_event(core.AutonomyEvents.START)
 
         # Do nothing.
         await asyncio.sleep(core.constants.EVENT_LOOP_DELAY)

core/vision/ar_tag_detector.py

@@ -34,10 +34,10 @@ async def async_ar_tag_detector():
             # Detect tags.
             TagDetector.detect_ar_tag(reg_img)
             # Filter tags.
-            if (core.states.state_machine.get_state_str() == "ApproachingGate"):
-                TagDetector.filter_ar_tags(angle_range=45, distance_range=15, valid_id_range=[0, 1, 2, 3, 4, 5])
-            else:
-                TagDetector.filter_ar_tags(angle_range=180, distance_range=15, valid_id_range=[0, 1, 2, 3, 4, 5])
+            # if (core.states.state_machine.get_state_str() == "ApproachingGate"):
+            #     TagDetector.filter_ar_tags(angle_range=45, distance_range=15, valid_id_range=[0, 1, 2, 3, 4, 5])
+            # else:
+            #     TagDetector.filter_ar_tags(angle_range=180, distance_range=15, valid_id_range=[0, 1, 2, 3, 4, 5])
             # Get and store tags.
             ar_tags = TagDetector.get_tags()
 

core/vision/zed_handler.py

@@ -220,8 +220,10 @@ def get_pose(self):
         tx = round(pose.get_translation(translation).get()[0], 3) / 1000
         ty = round(pose.get_translation(translation).get()[1], 3) / 1000
         tz = round(pose.get_translation(translation).get()[2], 3) / 1000
-        orientation = sl.Orientation()
-        ox, oy, oz = np.rad2deg(pose.get_orientation(orientation).get_rotation_matrix().get_euler_angles())
+        # Retrieve only frame synchronized data.
+        zed_imu_pose = sl.Transform()
+        self.zed.get_sensors_data(self.sensors_data, sl.TIME_REFERENCE.IMAGE)
+        ox, oy, oz = np.rad2deg(self.sensors_data.get_imu_data().get_pose(zed_imu_pose).get_orientation().get_rotation_matrix().get_euler_angles())
 
         # Wrap heading.
         if oy < 0:

docs/state_machine_matrix.csv

@@ -1,5 +1,5 @@
 Events,Idle,Navigating,SearchPattern,ApproachingMarker,Avoidance, ApproachingGate
-START,Navigating,Navigating,SearchPattern,ApproachingMarker,Avoidance, ApproachingGate
+START,Reversing,Navigating,SearchPattern,ApproachingMarker,Avoidance, ApproachingGate
 REACHED_GPS_COORDINATE,,SearchPattern,,,,
 MARKER_SEEN,,,ApproachingMarker,,,
 GATE_SEEN,,,ApproachingGate,,,

interfaces/nav_board.py

@@ -132,13 +132,15 @@ def location(self, force_absolute=False) -> Coordinate:
             if self._start_UTM is None:
                 # Get current GPS.
                 self._start_UTM = list(utm.from_latlon(self._location[0], self._location[1]))
+                # Get the current roll and yaw of camera. x and z axis. These need to be retained otherwise camera positional tracking will be off.
+                _, _, _, roll, _, yaw = location = core.vision.camera_handler.get_pose()
                 # Align set pose to current gps location and heading.
-                core.vision.camera_handler.set_pose(self._start_UTM[0], 0, self._start_UTM[1], 0, self._heading, 0)
+                core.vision.camera_handler.set_pose(0, 0, 0, roll, self._heading, yaw)
 
             # Get current pose of camera.
             location = core.vision.camera_handler.get_pose()
-            # Get zed x, y location. Actually Z.
-            x, y = location[0], location[2]
+            # Get zed x, y location. Longitude is actually Z in zed axis because of default coordinate frame.
+            x, y = self._start_UTM[0] + location[2], self._start_UTM[1] + location[0]
             # Convert back to GPS. Last two params are UTM zone.
             gps_current = utm.to_latlon(*(x, y, self._start_UTM[2], self._start_UTM[3]))
             gps_current = Coordinate(gps_current[0], gps_current[1])
@@ -176,9 +178,11 @@ def realign(self) -> None:
         Realigns the relative positioning to the absolute position.
         """
         # Get current GPS.
-        utm_current = list(utm.from_latlon(self._location[0], self._location[1]))
+        self._start_UTM = list(utm.from_latlon(self._location[0], self._location[1]))
+        # Get the current roll and yaw of camera. x and z axis. These need to be retained otherwise camera positional tracking will be off.
+        _, _, _, roll, _, yaw = location = core.vision.camera_handler.get_pose()
         # Align set pose to current gps location and heading.
-        core.vision.camera_handler.set_pose(utm_current[0], 0, utm_current[1], 0, self._heading, 0)
+        core.vision.camera_handler.set_pose(0, 0, 0, roll, self._heading, yaw)
 
         """
         OLD