Update simulation_modules.py

simulation_modules.py

@@ -524,83 +524,76 @@ def world_to_pixel(world_points: np.ndarray, grid_size_pixels: tuple, grid_size_
     return np.vstack((pixel_x, pixel_y)).T
 
 # ===================================================================
-#  الخطوة 2: دالة render_bev (محدثة للمرحلة الثانية: الخرائط المستقبلية)
 # ===================================================================
 def render_bev(
     active_tracks: List,
-    predicted_waypoints: np.ndarray,
+    waypoints_to_draw: np.ndarray,
     ego_pos_global: np.ndarray,
     ego_theta_global: float,
     pixels_per_meter: int = 20,
     grid_size_meters: tuple = (20, 20),
-    future_time_steps: tuple = (1.0, 2.0) # أزمنة التنبؤ المستقبلية
+    future_time_steps: tuple = (1.0, 2.0)
 ) -> Dict[str, np.ndarray]:
-    """
-    [المرحلة الثانية]
-    تنشئ خريطة للحظة الحالية (t0) وخرائط تنبؤ مستقبلية (t1, t2).
-    """
+
     side_m, fwd_m = grid_size_meters
     width_px, height_px = int(side_m * pixels_per_meter), int(fwd_m * pixels_per_meter)
 
-    # مصفوفة التحويل من إحداثيات العالم إلى إحداثيات المركبة
+    # ... (كود مصفوفة التحويل وإنشاء الخرائط يبقى كما هو)
     R_world_to_ego = np.array([[math.cos(ego_theta_global), math.sin(ego_theta_global)],
                                [-math.sin(ego_theta_global), math.cos(ego_theta_global)]])
-
-    # إنشاء قواميس للخرائط الفارغة
     bev_maps = {
         't0': np.zeros((height_px, width_px, 3), dtype=np.uint8),
         't1': np.zeros((height_px, width_px, 3), dtype=np.uint8),
         't2': np.zeros((height_px, width_px, 3), dtype=np.uint8)
     }
 
-    # --- 1. رسم الكائنات المتتبعة في الحاضر والمستقبل ---
+
     for track in active_tracks:
-        # --- أ. استخراج البيانات الحالية للكائن ---
-        pos_global = track.last_pos
-        yaw_rad_global = getattr(track, 'last_yaw', 0)
-        speed = getattr(track, 'speed', 0)
-        extent = getattr(track, 'last_extent', (1.0, 2.0)) # (الطول، العرض)
-
-        # --- ب. حساب الخصائص النسبية والرسم ---
-        relative_yaw_rad = yaw_rad_global - ego_theta_global
-        angle_deg = -90 - math.degrees(relative_yaw_rad)
-        width_px_obj = extent[1] * 2 * pixels_per_meter
-        length_px_obj = extent[0] * 2 * pixels_per_meter
-        box_size_px = (float(width_px_obj), float(length_px_obj))
-
-        # --- ج. رسم الكائن في الوقت الحالي (t0) ---
-        relative_pos_t0 = R_world_to_ego.dot(pos_global - ego_pos_global)
-        center_pixel_t0 = world_to_pixel(np.array([relative_pos_t0]), (width_px, height_px), grid_size_meters)[0]
-        box_points_t0 = cv2.boxPoints(((float(center_pixel_t0[0]), float(center_pixel_t0[1])), box_size_px, angle_deg))
-        cv2.drawContours(bev_maps['t0'], [box_points_t0.astype(np.int32)], 0, (0, 0, 255), 2) # أحمر
-
-        # --- د. التنبؤ بالمستقبل ورسمه ---
-        for i, t in enumerate(future_time_steps):
-            # حساب الإزاحة بناءً على السرعة والاتجاه والزمن
-            # (هذه العمليات تتم في الإطار المرجعي العالمي)
-            offset = np.array([math.cos(yaw_rad_global), math.sin(yaw_rad_global)]) * speed * t
-            future_pos_global = pos_global + offset
-
-            # تحويل الموقع المستقبلي إلى إحداثيات نسبية ثم إلى بكسلات
-            relative_pos_future = R_world_to_ego.dot(future_pos_global - ego_pos_global)
-            center_pixel_future = world_to_pixel(np.array([relative_pos_future]), (width_px, height_px), grid_size_meters)[0]
-
-            # رسم الصندوق المستقبلي (نفس الحجم والزاوية، ولكن بموقع ولون مختلفين)
-            key = f't{i+1}' # 't1', 't2', etc.
-            box_points_future = cv2.boxPoints(((float(center_pixel_future[0]), float(center_pixel_future[1])), box_size_px, angle_deg))
-            cv2.drawContours(bev_maps[key], [box_points_future.astype(np.int32)], 0, (255, 0, 128), 2) # بنفسجي
-
-    # --- 2. رسم مركبة الأنا (يجب رسمها على كل الخرائط) ---
-    ego_center_pixel = ((width_px / 2 ) + 20 , height_px - 10)
-    ego_size_px = (1.8 * pixels_per_meter, 2.0 * pixels_per_meter)
-    ego_box = cv2.boxPoints((ego_center_pixel, ego_size_px, -90))
+        pos_global=track.last_pos;yaw_rad_global=getattr(track,'last_yaw',0);
+        speed=getattr(track,'speed',0);extent=getattr(track,'last_extent',(1.0,2.0));
+        relative_yaw_rad=yaw_rad_global-ego_theta_global;angle_deg=90-math.degrees(relative_yaw_rad);
+        width_px_obj=extent[1]*2*pixels_per_meter;
+        length_px_obj=extent[0]*2*pixels_per_meter;
+        box_size_px=(float(width_px_obj),float(length_px_obj));
+        relative_pos_t0=R_world_to_ego.dot(pos_global-ego_pos_global);
+        center_pixel_t0=world_to_pixel(np.array([relative_pos_t0]),(width_px,height_px),grid_size_meters)[0];
+        box_points_t0=cv2.boxPoints(((float(center_pixel_t0[0]),float(center_pixel_t0[1])-40),box_size_px,angle_deg));
+        cv2.drawContours(bev_maps['t0'],[box_points_t0.astype(np.int32)],0,(0,0,255),2);
+        for i,t in enumerate(future_time_steps):
+            offset=np.array([math.cos(yaw_rad_global),math.sin(yaw_rad_global)])*speed*t;
+            future_pos_global=pos_global+offset;
+            relative_pos_future=R_world_to_ego.dot(future_pos_global-ego_pos_global);
+            center_pixel_future=world_to_pixel(np.array([relative_pos_future]),(width_px,height_px-40),grid_size_meters)[0];key=f't{i+1}';box_points_future=cv2.boxPoints(((float(center_pixel_future[0]),float(center_pixel_future[1])),box_size_px,angle_deg));
+            cv2.drawContours(bev_maps[key],[box_points_future.astype(np.int32)],0,(255,0,128),2);
+    ego_center_pixel=((width_px/2)+20,height_px);
+    ego_size_px=(1.8*pixels_per_meter,3.8*pixels_per_meter);
+    ego_box=cv2.boxPoints((ego_center_pixel,ego_size_px,0));
     for key in bev_maps:
-        cv2.drawContours(bev_maps[key], [ego_box.astype(np.int32)], 0, (0, 255, 255), -1) # أصفر
+        cv2.drawContours(bev_maps[key],[ego_box.astype(np.int32)],0,(0,255,255),-1);
+
+    # =========================================================================
+    # =========================================================================
+    if waypoints_to_draw is not None and waypoints_to_draw.size > 0:
+        waypoints_corrected = waypoints_to_draw.copy()
+        waypoints_corrected[:, 1] *= 1  # [Forward, Left] -> [Forward, -Left] which is [Forward, Right]
+
+
+        waypoints_pixels = world_to_pixel(
+            waypoints_corrected,
+            (width_px, height_px), 
+            grid_size_meters
+        )
 
-    # --- 3. رسم نقاط المسار المتوقعة (فقط على خريطة الحاضر t0) ---
-    if predicted_waypoints.size > 0:
-        waypoints_pixels = world_to_pixel(predicted_waypoints, (width_px+20, height_px), grid_size_meters)
-        cv2.polylines(bev_maps['t0'], [waypoints_pixels.astype(np.int32)], isClosed=False, color=(0, 255, 0), thickness=3)
+        for point in waypoints_pixels:
+            center = (int(point[0])+20, int(point[1]))
+            cv2.circle(
+                bev_maps['t0'],
+                center,
+                radius=3,           
+                color=(0, 255, 0), 
+                thickness=-1,      
+                lineType=cv2.LINE_AA
+            )
 
     return bev_maps
 # =========================================================