added crosswalks

pygpudrive/visualize/core.py

@@ -229,6 +229,27 @@ def _get_endpoints(self, x, y, length, yaw):
         start = center - np.array([length * np.cos(yaw), length * np.sin(yaw)])
         end = center + np.array([length * np.cos(yaw), length * np.sin(yaw)])
         return start, end
+
+    def _get_corners_polygon(self, x, y, length, width, orientation):
+        """Calculate the four corners of a speed bump (can be any) polygon."""
+        # Compute the direction vectors based on orientation
+        # print(length)
+        c = np.cos(orientation)
+        s = np.sin(orientation)
+        u = np.array((c, s))  # Unit vector along the orientation
+        ut = np.array((-s, c))  # Unit vector perpendicular to the orientation
+
+        # Center point of the speed bump
+        pt = np.array([x, y])
+
+        # corners
+        tl = pt + (length / 2) * u - (width / 2) * ut  
+        tr = pt + (length / 2) * u + (width / 2) * ut  
+        br = pt - (length / 2) * u + (width / 2) * ut  
+        bl = pt - (length / 2) * u - (width / 2) * ut  
+
+        # print([tl.tolist(), tr.tolist(), br.tolist(), bl.tolist()])
+        return [tl.tolist(), tr.tolist(), br.tolist(), bl.tolist()]
 
     def _plot_roadgraph(
         self,
@@ -253,6 +274,7 @@ def _plot_roadgraph(
                     or road_point_type == int(gpudrive.EntityType.RoadLane)
                     or road_point_type == int(gpudrive.EntityType.SpeedBump)
                     or road_point_type == int(gpudrive.EntityType.StopSign)
+                    or road_point_type == int(gpudrive.EntityType.CrossWalk)
 
                 ):
                     # Get coordinates and metadata
@@ -305,6 +327,16 @@ def _plot_roadgraph(
                                 linewidth=3.0,        
                                 alpha=1.0,            
                             )
+                    elif (road_point_type == int(gpudrive.EntityType.CrossWalk)):
+                        for x, y, length, width, orientation in zip(x_coords, y_coords, segment_lengths, segment_widths, segment_orientations):
+                            points = self._get_corners_polygon(x, y, length, width, orientation)
+                            utils.plot_crosswalk(
+                                points=points,
+                                ax=ax,
+                                facecolor="none",
+                                edgecolor='xkcd:bluish grey',
+                                alpha=0.4,
+                            )
 
                 else:
                     # Dots for other road point types

pygpudrive/visualize/utils.py

@@ -404,3 +404,29 @@ def plot_stop_sign(
         zorder=2,
     )
     ax.add_patch(p)
+
+def plot_crosswalk(
+    points,
+    ax: plt.Axes = None,
+    facecolor: str = None,
+    edgecolor: str = None,
+    alpha: float = None,
+):
+    if ax is None:
+        ax = plt.gca()
+    # override default config
+    facecolor = crosswalk_config['facecolor'] if facecolor is None else facecolor
+    edgecolor = crosswalk_config['edgecolor'] if edgecolor is None else edgecolor
+    alpha = crosswalk_config['alpha'] if alpha is None else alpha
+
+    p = Polygon(
+        points,
+        facecolor=facecolor,
+        edgecolor=edgecolor,
+        linewidth=2,
+        alpha=alpha,
+        hatch=r'//',
+        zorder=2,
+    )
+
+    ax.add_patch(p)