Current progress in COLAV, fixed some small errors, but does not try …

…to avoid anymore. Might be some mess ups

motion/colav/scripts/VO_math.py

@@ -21,11 +21,10 @@ class Approaches(Enum):
 
 
 class Obstacle:
-
     def __init__(self) -> None:
         self.vx = 0
         self.vy = 0
-        self.r = 0
+        self.radius = 0.5
         self.x = 0
         self.y = 0
         self.heading = 0
@@ -37,11 +36,11 @@ class VelocityObstacle:
     The Velocity Obstacle class
 
         A computational class used by the collision avoidance system 
-        to determine if a collison can occur, and how the UAV should respond
+        to determine if a collison can occur, and how the ASV should respond
 
         obstacle: An odometry of the object to avoid
         radius_o: The radius of obstacle
-        vessel: An odometry of the UAV vessel 
+        vessel: An odometry of the ASV vessel 
     """
 
     def __init__(self, vessel: Obstacle, obstacle: Obstacle) -> None:
@@ -52,22 +51,23 @@ def __init__(self, vessel: Obstacle, obstacle: Obstacle) -> None:
         self.left_angle = 0.0
         self.right_angle = 0.0
 
-        self.truncated_time = 5  #placeholder
+        self.truncated_time = 10  #placeholder
 
     def set_cone_angles(self) -> None:
         """
         Calculates the largest and smallest heading-angle where a collision can occur 
         """
-        theta_ro = math.atan2(self.obstacle.y - self.vessel.y,
-                              self.obstacle.x - self.vessel.x)
-        print("ob", self.vessel.r, self.obstacle.r)
-        distance = math.sqrt((self.obstacle.x - self.vessel.x) ** 2 + (self.obstacle.y - self.vessel.y) ** 2)
-        if distance == 0:
+        theta_ro = math.atan2(self.vessel.y - self.obstacle.y,
+                              self.vessel.x - self.obstacle.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])
+
+        if distance < (self.vessel.radius + self.obstacle.radius):
             # Handle the case where distance is zero
             # This could be setting theta_ray to some default value or skipping calculations
             return
         else:
-            theta_ray = math.asin((self.vessel.r + self.obstacle.r) / distance)
+            theta_ray = math.asin((self.vessel.radius + self.obstacle.radius) / distance)
 
         self.right_angle = theta_ro - theta_ray
         self.left_angle = theta_ro + theta_ray
@@ -79,10 +79,10 @@ def check_if_collision(self) -> bool:
 
         """
 
-        bouffer = 0
-        dvx = self.obstacle.vx - self.vessel.vx
-        dvy = self.obstacle.vy - self.vessel.vy
-        angle = math.atan2(-dvy, -dvx)
-        print("vels", dvx, dvy)
+        buffer = 0
+        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}")
 
-        return angle > self.right_angle - bouffer and angle < self.left_angle + bouffer
+        return angle > self.right_angle - buffer and angle < self.left_angle + buffer

motion/colav/scripts/colav_controller.py

@@ -40,6 +40,7 @@ def __init__(self):
     def vessel_callback(self, msg):
         self.vessel_odom = msg
         self.vessel = self.odometry_to_obstacle(msg)
+        #print("vessel odometry: ", self.vessel_odom)
         self.t = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
 
     def obst_callback(self, msg):
@@ -91,6 +92,8 @@ def gen_colav_data(self):
         # self.get_logger().info(f'Vessel at {self.vessel.x}, {self.vessel.y}')
         # self.get_logger().info(f'VO Angles: Left {VO.left_angle}, Right {VO.right_angle}')
 
+        #self.get_logger().info(f'Heading Angle: Left {self.vessel.heading}')
+
         zone = self.get_zone(closest_obst, self.vessel)
         self.get_logger().info(f'Zone: {zone}')
 
@@ -99,17 +102,22 @@ def gen_colav_data(self):
         elif zone == Zones.STOPNOW:
             return self.create_guidance_data(0, 0, self.vessel.heading, self.vessel_odom)
         elif zone == Zones.COLIMM and not VO.check_if_collision():
-            return self.create_guidance_data(self.vessel.speed, self.vessel.heading, self.vessel.heading, self.vessel_odom)
+            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)
 
         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
         return None
+
+        # TODO: Calculate proper psi_d
 
     def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom, is_colav=True):
         data = GuidanceData()
@@ -123,29 +131,33 @@ def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom, is_col
         #     z=vessel_odom.pose.pose.orientation.z,
         #     w=vessel_odom.pose.pose.orientation.w)
         orientation_list = [
-            vessel_odom.pose.pose.orientation.w,
             vessel_odom.pose.pose.orientation.x,
             vessel_odom.pose.pose.orientation.y,
-            vessel_odom.pose.pose.orientation.z
+            vessel_odom.pose.pose.orientation.z,
+            vessel_odom.pose.pose.orientation.w
         ]
         # _, _, yaw = self.quaternion_to_euler(orientation_q)
         yaw = quat2euler(orientation_list)[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
         dy = obstacle.y - vessel.y
-        buffer = 10 * math.pi / 180  # 10 degrees in radians
+        buffer = 30 * math.pi / 180  # 10 degrees in radians
         phi = math.atan2(dy, dx)
 
         if vessel.heading + buffer > phi > vessel.heading - buffer:
             return Approaches.FRONT
-        elif phi < vessel.heading - buffer:
+        elif self.normalize_angle(math.pi - (vessel.heading - buffer)) < phi < self.normalize_angle(vessel.heading - buffer):
             return Approaches.RIGHT
-        elif phi > vessel.heading + buffer:
+        elif self.normalize_angle(math.pi - (vessel.heading + buffer)) > phi > self.normalize_angle(vessel.heading + buffer):
             return Approaches.LEFT
         return Approaches.BEHIND
 
@@ -158,6 +170,8 @@ def get_zone(self, obstacle: Obstacle, vessel: Obstacle):
         return Zones.STOPNOW
 
 
+# TODO: add actual controller and not just generate references
+
 def main(args=None):
     rclpy.init()
     colav_controller = ColavController()

motion/colav/scripts/colav_sim.py

@@ -22,7 +22,7 @@ def __init__(self):
         self.speed = 0.5
         self.obstacle_pos_x = 5
         self.obstacle_pos_y = 5
-        self.obstacle_speed = 0.1
+        self.obstacle_speed = 0.5
         self.obstacle_heading = 5*np.pi/4
         self.goal_pos_x = 8
         self.goal_pos_y = 8
@@ -51,12 +51,13 @@ def step(self, speed, heading):
         odom_msg.pose.pose.orientation.y = quat[1]
         odom_msg.pose.pose.orientation.z = quat[2]
         odom_msg.pose.pose.orientation.w = quat[3]
+        #print(f"Simulator heading: {heading}")
         self.vessel_pub.publish(odom_msg)
 
     def colav_callback(self, msg: GuidanceData):
         if msg.is_colav:
             speed = msg.u
-            heading = msg.psi
+            heading = msg.psi_d   #msg.psi_d
             self.step(speed, heading)
         else:
             speed = self.speed
@@ -73,7 +74,7 @@ def obstacle_callback(self):
         obs_msg.pose.pose.position.y = self.obstacle_pos_y
         obs_msg.twist.twist.linear.x = obs_speed_x
         obs_msg.twist.twist.linear.y = obs_speed_y
-        quaternion = euler2quat(0, 0, self.obstacle_heading)
+        quaternion = self.heading_to_quaternion(self.obstacle_heading)
         # self.get_logger().info(f"Quat: {quaternion}")
         obs_msg.pose.pose.orientation.x = quaternion[0]
         obs_msg.pose.pose.orientation.y = quaternion[1]