Function plot_actor_bounding_box

src/utils/vis.py:27–80

Plot an actor bounding box centered on the actor's current location. Args: ax: Axes on which actor bounding box should be plotted. cur_location: Current location of the actor (2,). heading: Current heading of the actor (in radians). color: Desired color for the b

(
    ax: plt.Axes,
    cur_location: np.ndarray,
    heading: float,
    color: str,
    bbox_size: Tuple[float, float],
    alpha=1.0,
    label=None,
    zorder=50,
    fill=True,
)

def plot_actor_bounding_box(
    ax: plt.Axes,
    cur_location: np.ndarray,
    heading: float,
    color: str,
    bbox_size: Tuple[float, float],
    alpha=1.0,
    label=None,
    zorder=50,
    fill=True,
) -> None:
    """Plot an actor bounding box centered on the actor's current location.

    Args:
        ax: Axes on which actor bounding box should be plotted.
        cur_location: Current location of the actor (2,).
        heading: Current heading of the actor (in radians).
        color: Desired color for the bounding box.
        bbox_size: Desired size for the bounding box (length, width).
    """
    (bbox_length, bbox_width) = bbox_size

    # Compute coordinate for pivot point of bounding box
    d = np.hypot(bbox_length, bbox_width)
    theta_2 = math.atan2(bbox_width, bbox_length)
    pivot_x = cur_location[0] - (d / 2) * math.cos(heading + theta_2)
    pivot_y = cur_location[1] - (d / 2) * math.sin(heading + theta_2)

    vehicle_bounding_box = Rectangle(
        xy=(pivot_x, pivot_y),
        width=bbox_length,
        height=bbox_width,
        angle=np.degrees(heading),
        fc=color if fill else "none",
        ec=color,
        alpha=alpha,
        label=label,
        zorder=zorder,
    )
    ax.add_patch(vehicle_bounding_box)

    if bbox_length > 1.0:
        direction = (
            0.25 * bbox_size[0] * np.array([math.cos(heading), math.sin(heading)])
        )
        ax.arrow(
            cur_location[0],
            cur_location[1],
            direction[0],
            direction[1],
            color="white",
            zorder=zorder + 1,
            head_width=0.5,
        )


def plot_box(