diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index 79658b2..72f1aa2 100755 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -18,8 +18,8 @@ def __init__(self): super().__init__("colav_controller") self.declare_parameter('guidance_interface/colav_data_topic', 'guidance/collision_avoidance') - self.declare_parameter('stop_zone_radius', 0.9) - self.declare_parameter('colimm_max_radius', 2.0) + self.declare_parameter('stop_zone_radius', 1.5) + self.declare_parameter('colimm_max_radius', 7.0) stop_zone_radius = self.get_parameter('stop_zone_radius').value colimm_max_radius = self.get_parameter('colimm_max_radius').value @@ -109,8 +109,8 @@ def gen_colav_data(self): 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 + buffer = 20 * math.pi / 180 # 20 degrees + new_heading = VO.right_angle + 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 @@ -141,7 +141,7 @@ def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom, is_col def gen_approach(self, obstacle: Obstacle, vessel: Obstacle): dx = obstacle.x - vessel.x dy = obstacle.y - vessel.y - buffer = 30 * math.pi / 180 # 10 degrees in radians + buffer = math.pi / 6 # 30 degrees in radians phi = math.atan2(dy, dx) if vessel.heading + buffer > phi > vessel.heading - buffer: diff --git a/motion/colav/scripts/colav_sim.py b/motion/colav/scripts/colav_sim.py index ff07097..bd7f630 100755 --- a/motion/colav/scripts/colav_sim.py +++ b/motion/colav/scripts/colav_sim.py @@ -19,12 +19,12 @@ def __init__(self): self.pos_x = 0 self.pos_y = 0 - self.speed = 0.5 - self.obstacle_pos_x = 5 - self.obstacle_pos_y = 0 - self.obstacle_speed = 0.5 - self.obstacle_heading = 4*np.pi/4 - self.goal_pos_x = 8 + self.speed = 1.0 + self.obstacle_pos_x = 15 + self.obstacle_pos_y = 15 + self.obstacle_speed = 1.0 + self.obstacle_heading = 6*np.pi/4 + self.goal_pos_x = 20 self.goal_pos_y = 0 self.dt = 0.1