diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index 7f99cce1a..713857f11 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -44,23 +44,50 @@ def setReverse(self, reverse): raise NotImplementedError -class Walks: - """Mixin protocol for agents which can walk with a given direction and speed. +class Travels: + """Mixin protocol for agents which can travel with a given direction and speed. We provide a simplistic implementation which directly sets the velocity of the agent. This implementation needs to be explicitly opted-into, since simulators may provide a - more sophisticated API that properly animates pedestrians. + more sophisticated API for moving such agents. """ - def setWalkingDirection(self, heading): + def setTravelDirection(self, heading): velocity = Vector(0, self.speed).rotatedBy(heading) self.setVelocity(velocity) - def setWalkingSpeed(self, speed): + def setTravelSpeed(self, speed): velocity = speed * self.velocity.normalized() self.setVelocity(velocity) +class Walks(Travels): + """Mixin protocol for agents which can walk with a given direction and speed. + + This is the pedestrian-facing API used by scenarios and simulator interfaces. + It is also a backwards-compatible alias layer for `Travels`, so generic + direction and speed actions can control walkers. + + We provide a simplistic implementation which directly sets the velocity of the agent. + This implementation needs to be explicitly opted-into, since simulators may provide a + more sophisticated API that properly animates pedestrians. + """ + + # Legacy pedestrian API (simulator backends often override these). + def setWalkingDirection(self, heading): + super().setTravelDirection(heading) + + def setWalkingSpeed(self, speed): + super().setTravelSpeed(speed) + + # Bridge: ensure Travel actions on walkers go through walking overrides. + def setTravelDirection(self, heading): + self.setWalkingDirection(heading) + + def setTravelSpeed(self, speed): + self.setWalkingSpeed(speed) + + ## Actions available to all agents @@ -260,6 +287,39 @@ def applyTo(self, obj, sim): obj.setSteering(self.steer) +## Actions available to agents that travel by direction and speed + + +class TravelAction(Action): + """Abstract class for actions usable by agents which can travel. + + Such agents must implement the `Travels` protocol. + """ + + def canBeTakenBy(self, agent): + return isinstance(agent, Travels) + + +class SetTravelDirectionAction(TravelAction): + """Set the travel direction.""" + + def __init__(self, heading): + self.heading = heading + + def applyTo(self, obj, sim): + obj.setTravelDirection(self.heading) + + +class SetTravelSpeedAction(TravelAction): + """Set the travel speed.""" + + def __init__(self, speed): + self.speed = speed + + def applyTo(self, obj, sim): + obj.setTravelSpeed(self.speed) + + ## Actions available to agents that can walk @@ -280,7 +340,7 @@ def __init__(self, heading): self.heading = heading def applyTo(self, obj, sim): - obj.setWalkingDirection(self.heading) + obj.setTravelDirection(self.heading) class SetWalkingSpeedAction(WalkingAction): @@ -290,4 +350,4 @@ def __init__(self, speed): self.speed = speed def applyTo(self, obj, sim): - obj.setWalkingSpeed(self.speed) + obj.setTravelSpeed(self.speed) diff --git a/src/scenic/simulators/metadrive/model.scenic b/src/scenic/simulators/metadrive/model.scenic index 078c6fc33..dc6d43218 100644 --- a/src/scenic/simulators/metadrive/model.scenic +++ b/src/scenic/simulators/metadrive/model.scenic @@ -1,8 +1,8 @@ """Scenic world model for traffic scenarios in MetaDrive. -The model currently supports vehicles and pedestrians. It implements the +The model currently supports vehicles, pedestrians, and bicycles. It implements the basic :obj:`~scenic.domains.driving.model.Car` and `Pedestrian` classes from the :obj:`scenic.domains.driving` domain. -Vehicles and pedestrians support the basic actions and behaviors from the driving domain. +Vehicles, pedestrians, and bicycles support the basic actions and behaviors from the driving domain. The model defines several global parameters, whose default values can be overridden in scenarios using the ``param`` statement or on the command line using the @@ -118,6 +118,14 @@ class MetaDriveActor(DrivingObject): """ metaDriveActor: None + @property + def isPedestrian(self): + return False + + @property + def isBicycle(self): + return False + def setPosition(self, pos, elevation): position = scenicToMetaDrivePosition(pos, simulation().scenic_offset) self.metaDriveActor.set_position(position) @@ -176,15 +184,41 @@ class Car(Vehicle): class Pedestrian(Pedestrian, MetaDriveActor, Walks): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) - self._walking_direction = None - self._walking_speed = None + self._travel_direction = None + self._travel_speed = None @property def isPedestrian(self): return True - def setWalkingDirection(self, heading): - self._walking_direction = scenicToMetaDriveHeading(heading) + def setTravelDirection(self, heading): + self._travel_direction = scenicToMetaDriveHeading(heading) + + def setTravelSpeed(self, speed): + self._travel_speed = speed + + +class Bicycle(MetaDriveActor, Travels): + regionContainedIn: roadOrShoulder + position: new Point on roadOrShoulder + parentOrientation: roadDirection at self.position + + # match MetaDrive Cyclist defaults + width: 0.4 + length: 1.75 + height: 1.75 + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._travel_direction = None + self._travel_speed = None + + @property + def isBicycle(self): + return True + + def setTravelDirection(self, heading): + self._travel_direction = scenicToMetaDriveHeading(heading) - def setWalkingSpeed(self, speed): - self._walking_speed = speed + def setTravelSpeed(self, speed): + self._travel_speed = speed diff --git a/src/scenic/simulators/metadrive/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 76607d5bc..09bdcf49d 100644 --- a/src/scenic/simulators/metadrive/simulator.py +++ b/src/scenic/simulators/metadrive/simulator.py @@ -15,6 +15,7 @@ from metadrive.component.sensors.rgb_camera import RGBCamera from metadrive.component.sensors.semantic_camera import SemanticCamera +from metadrive.component.traffic_participants.cyclist import Cyclist from metadrive.component.traffic_participants.pedestrian import Pedestrian from metadrive.component.vehicle.vehicle_type import DefaultVehicle @@ -190,7 +191,7 @@ def createObjectInSimulator(self, obj): Create an object in the MetaDrive simulator. If it's the first object, it initializes the client and sets it up for the ego car. - For additional cars and pedestrians, it spawns objects using the provided position and heading. + For additional cars, pedestrians, and bicycles, it spawns objects using the provided position and heading. """ converted_position = utils.scenicToMetaDrivePosition( obj.position, self.scenic_offset @@ -264,6 +265,20 @@ def createObjectInSimulator(self, obj): metaDriveActor.set_velocity(direction, obj.speed) return + # For Bicycles + if obj.isBicycle: + metaDriveActor = self.client.engine.agent_manager.spawn_object( + Cyclist, + position=converted_position, + heading_theta=converted_heading, + ) + obj.metaDriveActor = metaDriveActor + self._attach_sensors(obj) + + direction = [math.cos(converted_heading), math.sin(converted_heading)] + metaDriveActor.set_velocity(direction, obj.speed) + return + # If the object type is unsupported, raise an error raise SimulationCreationError( f"Unsupported object type: {type(obj)} for object {obj}." @@ -281,16 +296,16 @@ def executeActions(self, allActions): action = obj._prepare_action() obj.metaDriveActor.before_step(action) else: - # For Pedestrians - if obj._walking_direction is None: - obj._walking_direction = utils.scenicToMetaDriveHeading(obj.heading) - if obj._walking_speed is None: - obj._walking_speed = obj.speed + # For Pedestrians and Bicycles + if obj._travel_direction is None: + obj._travel_direction = utils.scenicToMetaDriveHeading(obj.heading) + if obj._travel_speed is None: + obj._travel_speed = obj.speed direction = [ - math.cos(obj._walking_direction), - math.sin(obj._walking_direction), + math.cos(obj._travel_direction), + math.sin(obj._travel_direction), ] - obj.metaDriveActor.set_velocity(direction, obj._walking_speed) + obj.metaDriveActor.set_velocity(direction, obj._travel_speed) def step(self): start_time = time.monotonic() diff --git a/tests/simulators/metadrive/test_metadrive.py b/tests/simulators/metadrive/test_metadrive.py index 1c1ea9ad1..445ad733d 100644 --- a/tests/simulators/metadrive/test_metadrive.py +++ b/tests/simulators/metadrive/test_metadrive.py @@ -278,6 +278,46 @@ def test_initial_velocity_movement(getMetadriveSimulator): assert dx < -0.1, f"Expected car to move west (negative dx), but got dx = {dx}" +def test_bicycle_movement(getMetadriveSimulator): + simulator, openDrivePath, sumoPath = getMetadriveSimulator("Town01") + code = f""" + param map = r'{openDrivePath}' + param sumo_map = r'{sumoPath}' + + model scenic.simulators.metadrive.model + + behavior RideForward(): + while True: + take SetTravelDirectionAction(self.heading), SetTravelSpeedAction(0.5) + + behavior StopRiding(): + while True: + take SetTravelSpeedAction(0) + + behavior RideThenStop(): + do RideForward() for 2 steps + do StopRiding() for 2 steps + + ego = new Car at (30, 2) + bicycle = new Bicycle at (30, -2), + # with regionContainedIn None, + with behavior RideThenStop + + record bicycle.position as Pos + terminate after 4 steps + """ + scenario = compileScenic(code, mode2D=True) + scene = sampleScene(scenario) + simulation = simulator.simulate(scene) + series = simulation.result.records["Pos"] + + # moved at least once + assert series[0][1] != series[1][1] + + # after stopping, position should be (approximately) unchanged between last two samples + assert series[-1][1] == pytest.approx(series[-2][1], abs=0.05) + + def test_pedestrian_movement(getMetadriveSimulator): simulator, openDrivePath, sumoPath = getMetadriveSimulator("Town01") code = f"""