COLAV works!

motion/colav/scripts/VO_math.py

@@ -57,8 +57,8 @@ def set_cone_angles(self) -> None:
         """
         Calculates the largest and smallest heading-angle where a collision can occur 
         """
-        theta_ro = math.atan2(self.vessel.y - self.obstacle.y,
-                              self.vessel.x - self.obstacle.x)
+        theta_ro = math.atan2(self.obstacle.y - self.vessel.y, 
+                              self.obstacle.x - self.vessel.x)
         print("ob", self.vessel.radius, self.obstacle.radius)
         distance = np.linalg.norm([self.obstacle.x - self.vessel.x, self.obstacle.y - self.vessel.y])
 
@@ -79,10 +79,10 @@ def check_if_collision(self) -> bool:
 
         """
 
-        buffer = 0
+        buffer = math.pi / 12
         dvx = self.vessel.vx - self.obstacle.vx
         dvy = self.vessel.vy - self.obstacle.vy
         angle = math.atan2(dvy, dvx)
-        print(f"VO left angle: {self.left_angle}, VO right angle: {self.right_angle}, Velocity angle: {angle}")
+        # print(f"VO left angle: {self.left_angle}, VO right angle: {self.right_angle}, Velocity angle: {angle}")
 
         return angle > self.right_angle - buffer and angle < self.left_angle + buffer

motion/colav/scripts/colav_controller.py

@@ -103,15 +103,14 @@ def gen_colav_data(self):
             return self.create_guidance_data(0, 0, self.vessel.heading, self.vessel_odom)
         elif zone == Zones.COLIMM and not VO.check_if_collision():
             self.get_logger().info(f'No collision detected!')
-            return self.create_guidance_data(self.vessel.speed, self.vessel.heading, self.vessel.heading, self.vessel_odom, is_colav=True)
+            return self.create_guidance_data(self.vessel.speed, self.vessel.heading, self.vessel.heading, self.vessel_odom)
 
         approach = self.gen_approach(closest_obst, self.vessel)
         self.get_logger().info(f'Collition Detected!!!!')
 
         if approach in [Approaches.FRONT, Approaches.RIGHT]:
             buffer = math.pi / 6  # 30 degrees
             new_heading = VO.right_angle - buffer
-            #self.get_logger().info(f'New heading: {new_heading}, current heading: {self.vessel.heading}, VO right angle: {VO.right_angle}, buffer: {buffer}')
             return self.create_guidance_data(self.vessel.speed, new_heading, self.vessel.heading, self.vessel_odom)
         elif approach in [Approaches.BEHIND, Approaches.LEFT]:
             return None
@@ -125,27 +124,19 @@ def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom, is_col
         data.psi_d = float(psi_d)
         data.u = float(speed)  # Assuming this is the desired speed
         data.t = float(self.get_clock().now().seconds_nanoseconds()[0]) + float(self.get_clock().now().seconds_nanoseconds()[1]) * 1e-9
-        # orientation_q = Quaternion(
-        #     x=vessel_odom.pose.pose.orientation.x,
-        #     y=vessel_odom.pose.pose.orientation.y,
-        #     z=vessel_odom.pose.pose.orientation.z,
-        #     w=vessel_odom.pose.pose.orientation.w)
-        orientation_list = [
+
+        orientation_quat = [
             vessel_odom.pose.pose.orientation.x,
             vessel_odom.pose.pose.orientation.y,
             vessel_odom.pose.pose.orientation.z,
             vessel_odom.pose.pose.orientation.w
         ]
         # _, _, yaw = self.quaternion_to_euler(orientation_q)
-        yaw = quat2euler(orientation_list)[2]
+        yaw = quat2euler(orientation_quat)[2]
         self.get_logger().info(f'Current yaw: {yaw}')
         data.psi = float(yaw)
         data.is_colav = is_colav
         return data
-
-    def normalize_angle(self, angle):
-        """Normalize angle to be within the range [0, 2*pi)"""
-        return angle % (2 * math.pi)
 
     def gen_approach(self, obstacle: Obstacle, vessel: Obstacle):
         dx = obstacle.x - vessel.x
@@ -155,9 +146,9 @@ def gen_approach(self, obstacle: Obstacle, vessel: Obstacle):
 
         if vessel.heading + buffer > phi > vessel.heading - buffer:
             return Approaches.FRONT
-        elif self.normalize_angle(math.pi - (vessel.heading - buffer)) < phi < self.normalize_angle(vessel.heading - buffer):
+        elif (math.pi + vessel.heading - buffer) > phi > (vessel.heading + buffer):
             return Approaches.RIGHT
-        elif self.normalize_angle(math.pi - (vessel.heading + buffer)) > phi > self.normalize_angle(vessel.heading + buffer):
+        elif (vessel.heading - math.pi + buffer) < phi < (vessel.heading - buffer):
             return Approaches.LEFT
         return Approaches.BEHIND
 

motion/colav/scripts/colav_sim.py

@@ -21,11 +21,11 @@ def __init__(self):
         self.pos_y = 0
         self.speed = 0.5
         self.obstacle_pos_x = 5
-        self.obstacle_pos_y = 5
+        self.obstacle_pos_y = 0
         self.obstacle_speed = 0.5
-        self.obstacle_heading = 5*np.pi/4
+        self.obstacle_heading = 4*np.pi/4
         self.goal_pos_x = 8
-        self.goal_pos_y = 8
+        self.goal_pos_y = 0
 
         self.dt = 0.1