Search Pattern PR (#87)

* Added the methods for the new Search State.

* Was told to delete something, but not sure.

* Defined the Search State goal variable types.

* Created a class that deals with the calculations for the target coordinates.

* Created a class for the timer that tells the rover when to skip a coordinate if it is unreachable.

* Created a class for the timer that tells the rover when it has taken too long to find the target and it should exist the Search State.

* Created the action server for the Search state.

* Started to create the service for the camera with the fake FreeFall success, but we will need to change this since we are not using FreeFall anymore.

* Changed class name to UpperCamelCase.

* Changde lat1/lat2 and lon1/lon2 to more descriptive names.

* Changed comment to remember that the specific line might be deleted in the future.

* Wrote documentation for the class and all functions and included a link to where the formula was taken from. In-line comments were added so the code is readable, code and comments have been formatted so they are within a normal character range, and certain variable names were changed again to make them more descriptive.

* All python functions have been changed to snake_case.

* Removed print statements and made variable names more descriptive.

* Spaced out math operands to make things more readable.

* Wrote documentation for the timer classes and all functions. Added in-line comments for readability of confusing calculations.

* Added SearchState.action to CMakeLists and fixed code reference

* Renamed variable for the Earth's radius to be in all caps since its value does notchange.

* Added function headers, moved all calculations to one function, and removed unnecessary variables.

* Simplified the iterative logic of calculating the maximum time taken to find the target in one round to its closed form.

* Combined PointTimer.py and StateTimer.py into one file travel_timer.py. Added units for time, speed, and distance. Removed class heading from travel_timer.py and coord_calculations.py so they are more easily accessed in state_machine.py. Updated state_machine.py and search_action_server.py to call functions from travel_timer.py.

* Added return types in function headers.

* Added comment about motor power outputs.

* Renamed variable

* Fixed up errors in naming the SearchState* variables. Moved around the camera service code.

* Added file level comments.

---------

Co-authored-by: Arthur <[email protected]>

src/wr_logic_ai/CMakeLists.txt

@@ -70,6 +70,7 @@ add_action_files(
   FILES
   LongRange.action
   ShortRange.action
+  SearchState.action
 )
 
 # # Generate added messages and services with any dependencies listed here

src/wr_logic_ai/action/SearchState.action

@@ -0,0 +1,8 @@
+#goal definition
+float64 target_lat         # Info of current and target latitudes and longitudes
+float64 target_long
+uint8 dist
+---
+#result definition
+---
+#feedback definition

src/wr_logic_ai/action/ShortRange.action

@@ -1,5 +1,5 @@
 uint8 target_type
-uint8 TARGET_TYPE_GPS_ONLY = 0
+uint8 TARGET_TYPE_GPS_ONLY = 0 # Might delete later on.
 uint8 TARGET_TYPE_VISION = 1
 ---
 ---

src/wr_logic_ai/nodes/coord_calculations.py

@@ -0,0 +1,146 @@
+import math
+
+'''
+@ingroup wr_logic_ai
+@defgroup wr_logic_ai Coordinate Calculations
+@brief Calculates the coordinates the rover travels to when searching.
+
+Attributes(s):
+    EARTH_RADIUS - radius of the Earth
+
+Methods(s):
+    get_coords(start_lat, start_long, distance, num_vertices) - gets the list of coordinates
+    calc_dest_coord(curr_lat, curr_long, distance, bearing) - calculates the target coordinate
+'''
+
+EARTH_RADIUS = 6378100 # in meters
+
+def get_coords(start_lat, start_long, distance, num_vertices) -> list:
+    '''
+    Description: 'Main' method that gets the coordinates the rover will travel to in the current 
+        searching round based on the parameters. We only need to call this method.
+
+    Parameter(s): 
+        start_lat - the rover's starting latitude in the current searching round
+        start_long - the rover's starting longitude in the current searching round
+        distance - the distance traveled between the starting coordinate and the next 
+                   immediate coordinate
+        num_vertices - the maximum number of coordinates the rover could travel to
+
+    Return(s):
+        coords - list of coordinates (dictionaries) the rover will travel to during the search state
+    '''
+    cumulative_dist = distance
+    coords = []
+    bearing = 0
+    mult = 0
+
+    # create starting coordinate as the first coordinate
+    coords.append({'lat': start_lat, 'long': start_long})
+
+    for i in range(num_vertices):
+        # update the distance after it has been traveled twice to create the square spiral shape
+        if (i > 1) and (i % 2) == 0:
+            cumulative_dist += distance
+
+        coords.append(calc_dest_coord(coords[i]['lat'], coords[i]['long'], 
+                cumulative_dist, bearing))
+        bearing += 90
+
+    return coords
+
+def calc_dest_coord(curr_lat, curr_long, distance, bearing) -> dict:
+    '''
+    Description: Calculates the target latitude and longitude for each cardinal direction 
+        based on the current latitude and longitude, distance, and bearing.
+
+    The formula 'Destination point given distance and bearing from start point' from 
+    https://www.movable-type.co.uk/scripts/latlong.html. Based on these formulas, north 
+    has a bearing of 0°, east has a bearing of 90°, south has a bearing of 180°, 
+    and west has a bearing of 270°. 
+        
+    With the variables curr_lat, curr_long, distance, bearing, and EARTH_RADIUS, 
+        target_lat and target_long can be calculated using these formulas:
+        
+        target_lat = asin(
+            sin(curr_lat) 
+            ⋅ cos(distance / EARTH_RADIUS) 
+            + cos(curr_lat) 
+            ⋅ sin(distance / EARTH_RADIUS) 
+            ⋅ cos(bearing))
+        
+        target_long = curr_long + atan2(
+            sin(bearing) 
+            ⋅ sin(distance / EARTH_RADIUS) 
+            ⋅ cos(curr_lat), 
+            cos(distance / EARTH_RADIUS) 
+            − sin(curr_lat)^2))
+
+    Parameter(s):
+        curr_lat - the current latitude in degrees
+        curr_long - the current longitude in degrees
+        distance - the distance that will be traveled
+        bearing - the bearing/direction that will be traveled toward
+
+    Return(s):
+        coord - the newly calculated coordinate, containing:
+            target_lat - the newly calculated target latitude in degrees
+            target_long - the newly calculated target longitude in degrees
+            (what it took to get to the target latitude and longitude)
+            distance - the distance that was traveled
+            bearing - the bearing/direction that was traveled toward
+    '''
+    target_lat_rad = 0
+    target_long_rad = 0
+
+    # convert current latitude and longitude from degrees to radians
+    curr_lat_rad = math.radians(curr_lat)
+    curr_long_rad = math.radians(curr_long)
+
+    # north
+    if bearing % 360 == 0:
+        target_lat_rad = target_lat_rad = math.asin(
+            math.sin(curr_lat_rad) 
+            * math.cos(distance / EARTH_RADIUS) 
+            + math.cos(curr_lat_rad) 
+            * math.sin(distance / EARTH_RADIUS) 
+            * 1)
+        target_long_rad = curr_long_rad # does not change
+    # east
+    elif bearing % 360 == 90:
+        target_lat_rad = curr_lat_rad # does not change
+        target_long_rad = curr_long_rad + math.atan2(
+            1 
+            * math.sin(distance / EARTH_RADIUS) 
+            * math.cos(curr_lat_rad), 
+            math.cos(distance / EARTH_RADIUS) 
+            - math.sin(curr_lat_rad) ** 2)
+    # south
+    elif bearing % 360 == 180:
+        target_lat_rad = math.asin(
+            math.sin(curr_lat_rad) 
+            * math.cos(distance / EARTH_RADIUS) 
+            + math.cos(curr_lat_rad) 
+            * math.sin(distance / EARTH_RADIUS) 
+            * -1)
+        target_long_rad = curr_long_rad # does not change
+    # west
+    elif bearing % 360 == 270:
+        target_lat_rad = curr_lat_rad # does not change
+        target_long_rad = curr_long_rad + math.atan2(
+            -1 
+            * math.sin(distance / EARTH_RADIUS) 
+            * math.cos(curr_lat_rad), 
+            math.cos(distance / EARTH_RADIUS) 
+            - math.sin(curr_lat_rad) ** 2)
+    else:
+        return {}
+
+    # convert latitude and longitude back to degrees
+    target_lat = math.degrees(target_lat_rad)
+    target_long = math.degrees(target_long_rad)
+
+    # create the new coordinate
+    coord = {'lat': target_lat, 'long': target_long, 'distance': distance, 'bearing': bearing % 360}
+
+    return coord

src/wr_logic_ai/nodes/search_action_server.py

@@ -0,0 +1,54 @@
+import rospy
+import actionlib
+from wr_logic_ai.msg import SearchStateAction, SearchStateGoal
+import obstacle_avoidance
+import travel_timer
+
+'''
+@ingroup wr_logic_ai
+@defgroup wr_logic_ai Search Action Server
+@brief Tells the rover travel to the target coordinates and calls obstacle avoidance to make sure the rover can actually reach these coordinates.
+@details Enters a while loop that continually calls obstacle avoidance to update the target to the next coordinate in the list. 
+For each coordinate, if the time between the starting time and the current time go above the estimated travel time for that
+specific coordinate, the coordinate is assumed to be unreachable, and the rover will skip it and move on to the next coordinate.
+'''
+
+class SearchActionServer(object):
+    def __init__(self, name) -> None:
+        """
+        Initializes the action server
+
+        @param name (String): Name of the action server
+        """
+
+        self._action_name = name
+        obstacle_avoidance.initialize()
+        self._as = actionlib.SimpleActionServer(
+            self._action_name,
+            SearchStateAction,
+            execute_cb=self.execute_callback,
+            auto_start=False,
+        )
+        self._as.start()
+
+    def execute_callback(self, goal: SearchStateGoal):
+        """
+        Executes the long range obstacle avoidance code, and triggers the corresponding state machine event
+        depending on the result of the navigation
+
+        @param goal (SearchGoal): Goal for the navigation segment, which contains the GPS coordinates
+        of the target
+        """
+        start_time = rospy.get_rostime()
+        while (
+            rospy.get_rostime() - start_time < travel_timer.calc_point_time(goal.dist)
+            and not rospy.is_shutdown()
+        ):
+            if obstacle_avoidance.update_target(goal.target_lat, goal.target_long):
+                return self._as.set_succeeded()
+        return self._as.set_aborted()
+
+if __name__ == "__main__":
+    rospy.init_node("search_action_server")
+    server = SearchActionServer("SearchActionServer")
+    rospy.spin()

src/wr_logic_ai/nodes/search_pattern_client.py

@@ -0,0 +1,29 @@
+import rospy
+from wr_logic_ai.srv import SearchPatternService, SearchPatternServiceRequest, SearchPatternServiceResponse
+
+'''
+@ingroup wr_logic_ai
+@defgroup wr_logic_ai Search Pattern Client
+@brief The client for the camera that scans the rover's surroundings for the target object.
+@details This client is currently waiting for the response from the server and sending the 
+result (boolean), which seems a little redundant. This might be changed or completely 
+removed in the future.
+'''
+
+def main():
+    rospy.init_node("search_pattern_client")
+    search_pattern_service = rospy.ServiceProxy('search_pattern_service', SearchPatternService)
+    rate = rospy.Rate(10)
+    rospy.wait_for_service('object_detection')
+
+    while not rospy.is_shutdown():
+        try:
+            response:SearchPatternServiceResponse = search_pattern_service()
+            return response.output
+        except rospy.ServiceException as e:
+            print(f"Service call failed: {e}")
+
+        rate.sleep()
+
+if __name__ == '__main__':
+    main()

src/wr_logic_ai/nodes/search_pattern_server.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+
+import rospy
+from wr_logic_ai.srv import SearchPatternService, SearchPatternServiceRequest, SearchPatternServiceResponse
+import keyboard  # I guess we don't need this anymore since we're not using FreeFall
+
+'''
+@ingroup wr_logic_ai
+@defgroup wr_logic_ai Search Pattern Server
+@brief The server for the camera that scans the rover's surroundings for the target object.
+@details Currently, this is a simulated server where pressing 'w' is a success and 
+pressing 'l' is a fail. This was originally for testing the search pattern with FreeFall
+and will probably be changed in the future as we learn how to integrate the camera into
+the state machine software. 
+'''
+
+def search_pattern_handler(request: SearchPatternServiceRequest):
+    target_found = False
+
+    # simulate the camera detecting the target
+    if keyboard.is_pressed('w'):
+        target_found = True
+        return target_found # return what??
+    elif keyboard.is_pressed('l'):
+        return target_found # return what??
+
+# initialize the server node
+def search_pattern_server():
+    rospy.init_node('search_pattern_server')
+    s = rospy.Service('search_pattern_service', SearchPatternService, search_pattern_handler)
+    rospy.spin()
+
+if __name__ == '__main__':
+    search_pattern_server()

src/wr_logic_ai/nodes/state_machine/state_machine.py

@@ -29,7 +29,10 @@
     LongRangeGoal,
     LongRangeActionResult,
 )
+import coord_calculations, travel_timer
 from wr_logic_ai.msg import ShortRangeAction, ShortRangeGoal, ShortRangeActionResult
+from wr_logic_ai.msg import SearchStateAction, SearchStateGoal, SearchStateActionResult
+from wr_logic_ai.srv import SearchPatternService
 from wr_led_matrix.srv import (
     led_matrix as LEDMatrix,
     led_matrixRequest as LEDMatrixRequest,
@@ -80,6 +83,8 @@ class NavStateMachine(StateMachine):
     stLongRange = State()
     ## State representing that the robot is recovering from a error state
     stLongRangeRecovery = State()
+    ## State representing that the robot is searching for the target
+    stSearch = State()
     ## State representing that the robot is running in short range mode
     stShortRange = State()
     ## State representing that the robot has completed a task at a waypoint
@@ -92,14 +97,17 @@ class NavStateMachine(StateMachine):
     evSuccess = (
         stLongRange.to(stShortRange)
         | stLongRangeRecovery.to(stLongRange)
+        | stSearch.to(stShortRange) 
         | stShortRange.to(stWaypointSuccess)
     )
     ## Event representing an error condition being raised
     evError = (
         stLongRange.to(stLongRangeRecovery)
         | stLongRangeRecovery.to(stLongRangeRecovery)
+        | stSearch.to(stLongRange)
         | stShortRange.to(stLongRange)
     )
+
     ## Event representing a shortcircuit state transition from WaypointSuccess to LongRange
     evNotWaiting = stWaypointSuccess.to(stLongRange)
     ## Event representing an unconditional state transition from Init to LongRange
@@ -125,6 +133,8 @@ def __init__(self, mgr: CoordinateManager) -> None:
         # Initialization of messages to switch the mux
         self.mux_long_range = NavigationState()
         self.mux_long_range.state = NavigationState.NAVIGATION_STATE_LONG_RANGE
+        self.mux_search = NavigationState()
+        self.mux_search.state = NavigationState.NAVIGATION_STATE_SEARCH
         self.mux_short_range = NavigationState()
         self.mux_short_range.state = NavigationState.NAVIGATION_STATE_SHORT_RANGE
         super(NavStateMachine, self).__init__()
@@ -259,6 +269,54 @@ def on_enter_stLongRangeRecovery(self) -> None:
     def on_exit_stLongRangeRecovery(self) -> None:
         self.timer.shutdown()  # Shutdown mux timer
 
+    def _searchActionComplete(self, state: GoalStatus, _: SearchStateActionResult) -> None: # SearchActionResult
+        if state == GoalStatus.SUCCEEDED:
+            self.evSuccess()
+        else:
+            self.evError()
+
+    def on_enter_stSearch(self) -> None:
+        distance = 4
+        num_vertices = 22
+
+        print("\non enter stSearch")
+        rospy.loginfo("\non enter stSearch")
+        self.timer = rospy.Timer(rospy.Duration(0.2), lambda _: self.mux_pub.publish(self.mux_search))
+
+        # enter autonomous mode
+        set_matrix_color(COLOR_AUTONOMOUS)
+
+        self._client = actionlib.SimpleActionClient("SearchActionServer", SearchStateAction) # "SearchActionServer", SearchAction
+        self._client.wait_for_server()
+
+        coords = coord_calculations.get_coords(self._mgr.get_coordinate()["lat"], self._mgr.get_coordinate()["long"], distance, num_vertices)
+        SEARCH_TIMEOUT_TIME = travel_timer.calc_state_time() # default = 20 meters
+
+        i = 0
+        start_time = rospy.get_rostime()
+        while (
+            rospy.get_rostime() - start_time < SEARCH_TIMEOUT_TIME
+            and not rospy.is_shutdown()
+            and i < num_vertices
+        ):
+            if (i != 0): # skip starting point because rover is already there
+                goal = SearchStateGoal(target_lat=coords[i]["lat"], target_long=coords[i]["long"], dist=coords[i]['distance']) # SearchGoal
+                self._client.send_goal(goal, done_cb=lambda status, result: self._searchActionComplete(status, result))
+
+            # Camera Service - still not sure about integrating the camera with the state machine
+            camera_service = rospy.ServiceProxy('search_pattern_service', SearchPatternService)
+            camera_service.wait_for_service('search_pattern_service')
+
+            if camera_service:
+                break # what should be done when the target object is found? how should we enter ShortRange?
+
+            i += 1
+
+    def on_exit_stSearch(self) -> None:
+        print("Exiting Search")
+        rospy.loginfo("Exiting Search")
+        self.timer.shutdown()
+
     def _shortRangeActionComplete(
         self, state: GoalStatus, _: ShortRangeActionResult
     ) -> None: