From c0cd6987f6124da2a6585ab899df57e4e22fadc9 Mon Sep 17 00:00:00 2001 From: Alekskl Date: Fri, 2 Aug 2024 22:08:10 +0200 Subject: [PATCH] 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 | 34 ++++++++++++------------ motion/colav/scripts/VO_visualize.py | 0 motion/colav/scripts/colav_controller.py | 26 +++++++++++++----- motion/colav/scripts/colav_sim.py | 7 ++--- motion/colav/scripts/cone_visualizer.py | 0 5 files changed, 41 insertions(+), 26 deletions(-) mode change 100644 => 100755 motion/colav/scripts/VO_visualize.py mode change 100644 => 100755 motion/colav/scripts/cone_visualizer.py diff --git a/motion/colav/scripts/VO_math.py b/motion/colav/scripts/VO_math.py index c0d13ac8..aadd1cda 100644 --- a/motion/colav/scripts/VO_math.py +++ b/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 \ No newline at end of file + return angle > self.right_angle - buffer and angle < self.left_angle + buffer \ No newline at end of file diff --git a/motion/colav/scripts/VO_visualize.py b/motion/colav/scripts/VO_visualize.py old mode 100644 new mode 100755 diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index 520fdc25..d2e2f075 100755 --- a/motion/colav/scripts/colav_controller.py +++ b/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,10 +131,10 @@ 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] @@ -134,18 +142,22 @@ def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom, is_col 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() diff --git a/motion/colav/scripts/colav_sim.py b/motion/colav/scripts/colav_sim.py index 289703a0..1453dc6a 100755 --- a/motion/colav/scripts/colav_sim.py +++ b/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] diff --git a/motion/colav/scripts/cone_visualizer.py b/motion/colav/scripts/cone_visualizer.py old mode 100644 new mode 100755