fixed sign error

vortexntnu · Aug 4, 2024 · bf6d335 · bf6d335
1 parent 38ccf64
commit bf6d335
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 11 deletions.
diff --git 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
@@ -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