Merge pull request #3 from ukleon123/school

School

Author: ukleon123

controllers/On_Simulation/DWA/dwa.py

@@ -58,8 +58,8 @@ def _calc_dynamic_window(self, x):
         Vd = [
             x[3] - self.max_accel * self.dt,
             x[3] + self.max_accel * self.dt,
-            x[4] - self.max_delta_yaw_rate * self.dt,
-            x[4] + self.max_delta_yaw_rate * self.dt
+            -self.max_delta_yaw_rate * self.dt,
+            self.max_delta_yaw_rate * self.dt
         ]
 
         # Final dynamic window (intersection of constraints)
@@ -93,6 +93,8 @@ def _calc_control_and_trajectory(self, x, dw, goal, obstacles):
 
         valid_trajectories = 0
 
+        # print(f"[DEBUG] 현재: x={x[0]:.2f}, y={x[1]:.2f}, yaw={x[2]:.2f}")  # debug
+        
         # Sample velocities in dynamic window
         for v in np.arange(dw[0], dw[1], self.v_resolution):
 
@@ -103,12 +105,16 @@ def _calc_control_and_trajectory(self, x, dw, goal, obstacles):
 
             for omega in omegas:
 
+                # print("=====") #debug
+                
                 # Predict trajectory
                 trajectory = self._predict_trajectory(x_init, v, omega)
 
                 # Calculate costs
                 obstacle_cost = self._calc_obstacle_cost(trajectory, obstacles)
-
+                if obstacle_cost == 0.0:
+                    continue
+                
                 speed_cost = self._calc_speed_cost(trajectory)
                 goal_cost = self._calc_goal_cost(trajectory, goal)
                 curvature_cost = self._calc_curvature_cost(trajectory, goal)
@@ -120,17 +126,23 @@ def _calc_control_and_trajectory(self, x, dw, goal, obstacles):
                     self.obstacle_cost_gain * obstacle_cost +
                     self.curvature_cost_gain * curvature_cost
                 )
-                if obstacle_cost == 0.0:
-                    continue
+                
                 # Skip collision trajectories
                 valid_trajectories += 1
-            
+                
+                
+                # print(f"omega: {omega:.4f}, total_cost: {total_cost:.4f}") #debug
+                # print(f"trajectory: {trajectory[-1, 0]}, {trajectory[-1, 1]}, {trajectory[-1, 2]}") #debug
+                #print(f"costs: speed: {speed_cost:.4f}, goal: {goal_cost:.4f}, obstacle: {obstacle_cost:.4f}, curvature: {curvature_cost:.4f}") #debug
+                
                 # Update best trajectory
                 if total_cost > max_cost:
                     max_cost = total_cost
                     self.best_u = [v, omega]
                     best_trajectory = trajectory
-
+        
+        # print(f"best u: {self.best_u[0]}, {self.best_u[1]} = max_cost: {max_cost:.4f}") #debug
+        # print("--------------------------------------------") #debug
         return self.best_u, best_trajectory
 
     def _predict_trajectory(self, x_init, v, omega):
@@ -152,29 +164,50 @@ def _predict_trajectory(self, x_init, v, omega):
 
         return trajectory
 
+
     def _calc_speed_cost(self, trajectory):
         """Speed cost - encourages higher speeds (normalized)"""
         if len(trajectory) == 0:
             return 0.0
         # Normalize speed cost (higher speed = higher cost value for maximization)
         return trajectory[-1, 3] / self.max_speed
 
-    def _calc_goal_cost(self, trajectory, goal):
-        """Goal cost - encourages moving toward goal"""
+    def _calc_goal_cost(self, trajectory, goal, alpha=0.3, beta=0.7):
+        """
+        혼합 goal cost 계산 함수 (높을수록 좋은 값)
+        - alpha: 방향 cost 가중치
+        - beta: 거리 cost 가중치
+        """
         if len(trajectory) == 0:
-            return 1.0
-            
-        # Distance to goal from final trajectory point
-        dx = goal[0] - trajectory[-1, 0]
-        dy = goal[1] - trajectory[-1, 1]
-        
-        # Heading error - angle between robot heading and direction to goal
+            return 1.0  # 최악의 경우
+
+        x, y, theta = trajectory[-1, 0], trajectory[-1, 1], trajectory[-1, 2]
+
+        dx = goal[0] - x
+        dy = goal[1] - y
+        distance = np.hypot(dx, dy)
+        #print(f"[DEBUG] Goal position: {goal}, Robot position: ({x:.2f}, {y:.2f}), Distance: {distance:.2f}")  # debug
+
+        # 거리 기반 cost (가까울수록 높음)
+        distance_cost = 1.0 / (distance + 1e-6)
+
+        # 각도 차이 계산 함수
+        def angle_diff(a, b):
+            diff = (a - b + np.pi) % (2 * np.pi) - np.pi
+            return diff
+
+        # 방향 기반 cost
         goal_angle = np.arctan2(dy, dx)
-        heading_error = goal_angle - trajectory[-1, 2]
-        heading_error = np.abs(heading_error) % (2 * np.pi)
-        heading_cost = np.pi - heading_error
-        # Combined cost: distance + heading (both should be minimized, so we use 1/cost format)
-        return heading_cost / np.pi
+        heading_error = abs(angle_diff(goal_angle, theta))
+        heading_cost = (np.pi - heading_error) / np.pi  # 1에 가까울수록 방향이 정확함
+        
+        # print(f"[DEBUG] Goal cost - Heading: {heading_cost:.4f}, Distance: {distance_cost:.4f}")  # debug
+
+        # 혼합 cost
+        total_cost = alpha * heading_cost + beta * distance_cost
+        return total_cost
+
+
 
     # def _calc_obstacle_cost(self, trajectory, obstacles):
     #     """Obstacle cost - avoids obstacles (optimized for ROSbot2)"""
@@ -229,8 +262,13 @@ def _calc_obstacle_cost(self, trajectory, obstacles):
             if len(distances) > 0:
                 min_dist = np.min(distances)
                 if min_dist <= safety_radius:
+                    #print(f"[WARNING] 장애물이 매우 가까움! 거리: {min_dist:.2f} m (safety_radius={safety_radius:.2f})")
                     return 0.0
 
+                elif min_dist <= safety_radius + 0.2:
+                    #print(f"[INFO] 장애물 접근 감지: 거리 {min_dist:.2f} m")
+                    return max(min_dist / (safety_radius + 0.2), 0.01)
+                
                 elif min_dist < min_distance:
                     min_distance = min_dist
 

controllers/On_Simulation/DWA/dwa_config.py

@@ -8,21 +8,21 @@ def __init__(self):
         # Motion parameters based on ROSbot2 capabilities
         self.dt = 0.032  # [s] Time tick for motion prediction (32ms - Webots timestep)
         self.max_accel = 1.0  # [m/s²] Maximum acceleration
-        self.max_speed = 1.0  # [m/s] Maximum linear velocity (ROSbot2 spec)
+        self.max_speed = 5.0  # [m/s] Maximum linear velocity (ROSbot2 spec)
         self.min_speed = 0.3  # [m/s] Minimum speed (can stop completely)
         self.max_yaw_rate = 7.33  # [rad/s] Maximum angular velocity (ROSbot2 spec)
-        self.max_delta_yaw_rate = 6.0  # [rad/s²] Maximum angular acceleration
+        self.max_delta_yaw_rate = 10.0  # [rad/s²] Maximum angular acceleration
 
         # Sampling resolutions - optimized for ROSbot2
         self.v_resolution = 0.1  # [m/s] Linear velocity sampling resolution
-        self.predict_time = 3  # [s] Prediction horizon (reduced for faster computation)
+        self.predict_time = 2  # [s] Prediction horizon (reduced for faster computation)
         self.yaw_rate_resolution = 0.1  # [rad/s] Angular velocity sampling resolution
 
         # Robot stuck prevention
         self.robot_stuck_flag_cons = 0.01  # Threshold for stuck detection
 
         # Safety margins
-        self.safety_margin = 0.3  # [m] Additional safety margin around robot
+        self.safety_margin = 0.35  # [m] Additional safety margin around robot
         self.collision_radius = 0.140