programmed navigation mission so that the boat goes to the closest re…

…d and green buoy set, and modified the world files to not include the white buoy start gates

NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py

@@ -24,6 +24,7 @@
 from .killed import Killed
 from .move import Move
 from .navigation import Navigation
+from .navigation_gatefinder import NavigationGatefinder
 from .navigator import NaviGatorMission
 from .obstacle_avoid import ObstacleAvoid
 from .pinger import PingerMission

NaviGator/mission_control/navigator_missions/navigator_missions/navigation_gatefinder.py

@@ -6,13 +6,17 @@
 from typing import ClassVar
 
 import axros
+import numpy as np
+import rospy
 from geometry_msgs.msg import Pose
 from mil_msgs.msg import PerceptionObject
 from std_srvs.srv import SetBoolRequest
 from tf.transformations import quaternion_from_euler
 
 from .navigator import NaviGatorMission
 
+FAILURE_THRESHOLD = 2000
+
 
 class BuoyColor(Enum):
     RED = auto()
@@ -35,12 +39,19 @@ def from_label(cls, label: str):
 class Buoy:
     color: BuoyColor
     pose: Pose
+    id: int
 
     @classmethod
     def from_perception_object(cls, perception_object: PerceptionObject):
         return cls(
             color=BuoyColor.from_label(perception_object.labeled_classification),
             pose=perception_object.pose,
+            id=perception_object.id,
+        )
+
+    def xyz(self):
+        return np.array(
+            [self.pose.position.x, self.pose.position.y, self.pose.position.z],
         )
 
     def distance(self, pose: Pose) -> float:
@@ -59,13 +70,14 @@ class Gate:
     required_right_color: ClassVar[BuoyColor]
 
     @property
-    def mid_pose(self) -> Pose:
-        mid_x = (self.left_buoy.pose.position.x + self.right_buoy.pose.position.y) / 2
+    def mid_pose(self):
+        # Calculate the midpoint of the found buoys
+        mid_x = (self.left_buoy.pose.position.x + self.right_buoy.pose.position.x) / 2
         mid_y = (self.left_buoy.pose.position.y + self.right_buoy.pose.position.y) / 2
         slope = (self.left_buoy.pose.position.x - self.right_buoy.pose.position.x) / (
             self.left_buoy.pose.position.y - self.right_buoy.pose.position.y
         )
-        angle = math.atan(slope)
+        angle = math.atan(slope) + math.pi / 2
         pose = Pose()
         pose.position.x = mid_x
         pose.position.y = mid_y
@@ -74,7 +86,7 @@ def mid_pose(self) -> Pose:
         pose.orientation.y = quat[1]
         pose.orientation.z = quat[2]
         pose.orientation.w = quat[3]
-        return pose
+        return (mid_x, mid_y, 0), quat
 
     @classmethod
     def other_color(cls, color_one: BuoyColor) -> BuoyColor:
@@ -94,18 +106,22 @@ class MiddleGate(Gate):
 @dataclass
 class StartGate(Gate):
     required_left_color = BuoyColor.RED
-    required_right_color = BuoyColor.WHITE
+    required_right_color = BuoyColor.GREEN
 
 
 @dataclass
 class EndGate(Gate):
     required_left_color = BuoyColor.RED
-    required_right_color = BuoyColor.WHITE
+    required_right_color = BuoyColor.GREEN
 
 
 class GateClassifier:
     def __init__(self, mission: NavigationGatefinder):
         self.mission = mission
+        self.trajectory = []
+        self.ids_invesitigated = set()
+        self.failures_to_move = 0
+        self.midpoint_lengths = []
 
     def _nearby_buoys(self, buoy: Buoy) -> list[Buoy]:
         """
@@ -117,6 +133,188 @@ def _nearby_buoys(self, buoy: Buoy) -> list[Buoy]:
     def mark_completed(self, gate: Gate) -> None:
         pass
 
+    async def get_next_gate(self, boat_pose) -> Gate | int:
+        # Get all database objects
+        objects: list[PerceptionObject] = (
+            await self.mission.database_query(name="all")
+        ).objects
+
+        # get nearest red and nearest green buoys
+        nearest_red: None | Buoy = None
+        nearest_green: None | Buoy = None
+        for db_object in objects:
+            # Could we do this more efficiently with a different database request?
+            DB_BUOY = Buoy.from_perception_object(db_object)
+            if (
+                db_object.labeled_classification == "red_cylinder"
+                and DB_BUOY.id not in self.ids_invesitigated
+            ):
+                if nearest_red:
+                    distance = np.linalg.norm(DB_BUOY.xyz() - boat_pose)
+                    nearest_red_distance = np.linalg.norm(nearest_red.xyz() - boat_pose)
+                    nearest_red = (
+                        DB_BUOY if distance < nearest_red_distance else nearest_red
+                    )
+                else:
+                    nearest_red = DB_BUOY
+
+            if (
+                db_object.labeled_classification == "green_cylinder"
+                and DB_BUOY.id not in self.ids_invesitigated
+            ):
+                if nearest_green:
+                    distance = np.linalg.norm(DB_BUOY.xyz() - boat_pose)
+                    nearest_green_distance = np.linalg.norm(
+                        nearest_green.xyz() - boat_pose,
+                    )
+                    nearest_green = (
+                        DB_BUOY if distance < nearest_green_distance else nearest_green
+                    )
+                else:
+                    nearest_green = DB_BUOY
+
+        # calculate midpoint between both points
+        # if ONLY green then go to a point 5 meters from the right of the green buoy
+        # if ONLY red then go to a point 5 meters from the left of the red buoy
+
+        # mark those buoys as investigated
+        mid_point = None
+        if nearest_green and nearest_red:
+            self.ids_invesitigated.add(nearest_red.id)
+            self.ids_invesitigated.add(nearest_green.id)
+            mid_x = (nearest_green.pose.position.x + nearest_red.pose.position.x) / 2
+            mid_y = (nearest_green.pose.position.y + nearest_red.pose.position.y) / 2
+            mid_point = (mid_x, mid_y, 0)
+            self.trajectory.append(mid_point)
+
+        # missing red
+        elif nearest_green:
+            rospy.logerr(
+                f"Could not find red for green({nearest_green.id}), here is the boats pos:\n{boat_pose}",
+            )
+            # Get nearest red to the green
+            objects: list[PerceptionObject] = (
+                await self.mission.database_query(name="all")
+            ).objects
+            nearest_red = None
+            for db_object in objects:
+                # Could we do this more efficiently with a different database request?
+                rospy.logerr(db_object.labeled_classification)
+                DB_BUOY = Buoy.from_perception_object(db_object)
+                if (
+                    db_object.labeled_classification == "red_cylinder"
+                    and DB_BUOY.id not in self.ids_invesitigated
+                ):
+                    if nearest_red:
+                        distance = np.linalg.norm(DB_BUOY.xyz() - nearest_green.xyz())
+                        nearest_red_distance = np.linalg.norm(
+                            nearest_red.xyz() - nearest_green.xyz(),
+                        )
+                        nearest_red = (
+                            DB_BUOY if distance < nearest_red_distance else nearest_red
+                        )
+                    else:
+                        nearest_red = DB_BUOY
+            # Set midpoint if nearest red was found
+            if nearest_red:
+                mid_x = (
+                    nearest_green.pose.position.x + nearest_red.pose.position.x
+                ) / 2
+                mid_y = (
+                    nearest_green.pose.position.y + nearest_red.pose.position.y
+                ) / 2
+                mid_point = (mid_x, mid_y, 0)
+                self.trajectory.append(mid_point)
+
+        # missing green
+        elif nearest_red:
+            rospy.logerr(
+                f"Could not find green for red({nearest_red.id}), here is the boats pos:\n{boat_pose}",
+            )
+            # Get nearest green to the red
+            objects: list[PerceptionObject] = (
+                await self.mission.database_query(name="all")
+            ).objects
+            nearest_green = None
+            for db_object in objects:
+                DB_BUOY = Buoy.from_perception_object(db_object)
+                if (
+                    db_object.labeled_classification == "green_cylinder"
+                    and DB_BUOY.id not in self.ids_invesitigated
+                ):
+                    if nearest_green:
+                        distance = np.linalg.norm(DB_BUOY.xyz() - nearest_red.xyz())
+                        nearest_green_distance = np.linalg.norm(
+                            nearest_green.xyz() - nearest_red.xyz(),
+                        )
+                        nearest_green = (
+                            DB_BUOY
+                            if distance < nearest_green_distance
+                            else nearest_green
+                        )
+                    else:
+                        nearest_green = DB_BUOY
+            # Set midpoint if nearest red was found
+            if nearest_green:
+                mid_x = (
+                    nearest_green.pose.position.x + nearest_red.pose.position.x
+                ) / 2
+                mid_y = (
+                    nearest_green.pose.position.y + nearest_red.pose.position.y
+                ) / 2
+                mid_point = (mid_x, mid_y, 0)
+                self.trajectory.append(mid_point)
+
+        rospy.logerr(mid_point)
+
+        # return next move point
+        if mid_point and nearest_green and nearest_red:
+            self.failures_to_move = 0
+            rospy.logerr(f"nearest red: {nearest_red.id}")
+            rospy.logerr(f"nearest green: {nearest_green.id}")
+            self.trajectory.append(mid_point)
+            self.midpoint_lengths.append(
+                math.sqrt(
+                    (nearest_green.pose.position.x - mid_point[0]) ** 2
+                    + (nearest_green.pose.position.y - mid_point[1]) ** 2,
+                ),
+            )
+            return Gate(nearest_red, nearest_green)
+
+        if self.failures_to_move > FAILURE_THRESHOLD:
+            rospy.logerr("FAILURE THRESHOLD REACHED")
+            return -1
+        #     from sklearn.preprocessing import PolynomialFeatures
+        #     from sklearn.linear_model import LinearRegression
+        #     import matplotlib.pyplot as plt
+
+        #     points=np.array(self.trajectory)
+        #     x = points[:, 0].reshape(-1, 1)  # Reshape for sklearn
+        #     y = points[:, 1]
+        #     # Follow the trajectory until you reach the farthest cylinder that is within the
+        #     poly = PolynomialFeatures(degree=2)  # You can change the degree as needed
+        #     x_poly = poly.fit_transform(x)
+
+        #     # Fit the polynomial regression model
+        #     model = LinearRegression()
+        #     model.fit(x_poly, y)
+        #     x_pred = np.linspace(min(x), max(x), 100)
+
+        #     # Predict using the transformed features
+        #     y_pred = model.predict(poly.transform(x_pred))
+
+        #     # Plot the results
+        #     plt.scatter(x, y, color='blue', label='Original Points')
+        #     plt.plot(x_pred, y_pred, color='red', label='Predicted Trajectory')
+        #     plt.xlabel('X Position')
+        #     plt.ylabel('Y Position')
+        #     plt.title('Robot Trajectory Prediction (Polynomial)')
+        #     plt.legend()
+        #     plt.show()
+
+        self.failures_to_move += 1
+        return 0
+
     async def available_gates(self) -> list[Gate]:
         """
         Returns a list of the gates that still need to be traversed.
@@ -183,9 +381,14 @@ async def run(self, parameters):
         await self.set_classifier_enabled(SetBoolRequest(True))
         # start_pose = await self.tx_pose()
         classifier = GateClassifier(self)
+
         while True:
-            gates = await classifier.available_gates()
-            self.send_feedback(f"Found {len(gates)} gates...")
-            for gate in gates:
-                self.send_feedback(f"Traversing {gate}...")
-                await self.move.set_position(gate.mid_pose).look_at(gate.mid_pose).go()
+            rospy.logerr(classifier.ids_invesitigated)
+            next_gate = await classifier.get_next_gate(self.pose[0])
+            if next_gate is Gate:
+                await self.move.set_position(next_gate.mid_pose[0]).set_orientation(
+                    next_gate.mid_pose[1],
+                ).go()
+            elif next_gate == -1:
+                rospy.loginfo("All Done!")
+                break

NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_clear.world

@@ -85,7 +85,6 @@
       <goal_pose>-33.722718 150.674031 0.0</goal_pose>
       <initial_state_duration>10</initial_state_duration>
       <ready_state_duration>10</ready_state_duration>
-      <running_state_duration>300</running_state_duration>
       <release_joints>
         <joint>
           <name>wamv_external_pivot_joint</name>
@@ -208,16 +207,6 @@
       </ambient>
     </scene>
     <!-- The navigation course -->
-    <include>
-      <name>buoy_start_r</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-520.338 168.871 0 0 0 -1.44</pose>
-    </include>
-    <include>
-      <name>buoy_start_l</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-531.2 176.364 0 0 0 -1.44</pose>
-    </include>
     <include>
       <name>buoy_green1</name>
       <uri>model://mb_marker_buoy_green</uri>
@@ -278,15 +267,5 @@
       <uri>model://mb_marker_buoy_red</uri>
       <pose>-500.408539 209.651611 0 0 0 -1.44</pose>
     </include>
-    <include>
-      <name>buoy_end_r</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-490.633240 200.413437 0 0 0 -1.44</pose>
-    </include>
-    <include>
-      <name>buoy_end_l</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-492.262939 209.520996 0 0 0 -1.44</pose>
-    </include>
   </world>
 </sdf>

NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_obs.world

@@ -208,16 +208,6 @@
       </ambient>
     </scene>
     <!-- The navigation course -->
-    <include>
-      <name>buoy_start_r</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-520.338 168.871 0 0 0 -1.44</pose>
-    </include>
-    <include>
-      <name>buoy_start_l</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-531.2 176.364 0 0 0 -1.44</pose>
-    </include>
     <include>
       <name>buoy_green1</name>
       <uri>model://mb_marker_buoy_green</uri>
@@ -278,16 +268,6 @@
       <uri>model://mb_marker_buoy_red</uri>
       <pose>-500.408539 209.651611 0 0 0 -1.44</pose>
     </include>
-    <include>
-      <name>buoy_end_r</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-490.633240 200.413437 0 0 0 -1.44</pose>
-    </include>
-    <include>
-      <name>buoy_end_l</name>
-      <uri>model://mb_marker_buoy_white</uri>
-      <pose>-492.262939 209.520996 0 0 0 -1.44</pose>
-    </include>
     <include>
       <name>buoy_black1</name>
       <uri>model://mb_round_buoy_black</uri>