From c5fd4750e15ea5335ec96f69798394f345d3703b Mon Sep 17 00:00:00 2001 From: rlyu Date: Tue, 30 Apr 2024 10:33:17 -0700 Subject: [PATCH 1/4] Adding AgentProperties field for DRIVE and INITIALIZE --- invertedai/api/drive.py | 21 +++- invertedai/api/initialize.py | 23 ++++ invertedai/api/mock.py | 6 + invertedai/common.py | 29 +++++ tests/test_drive.py | 151 ++++++++++++++++++++++-- tests/test_initialize.py | 215 +++++++++++++++++++++++++++++++---- 6 files changed, 409 insertions(+), 36 deletions(-) diff --git a/invertedai/api/drive.py b/invertedai/api/drive.py index 66fb42e8..9efd8189 100644 --- a/invertedai/api/drive.py +++ b/invertedai/api/drive.py @@ -17,6 +17,7 @@ Image, InfractionIndicators, AgentAttributes, + AgentProperties, TrafficLightStatesDict, LightRecurrentStates, LightRecurrentState, @@ -52,7 +53,8 @@ class DriveResponse(BaseModel): def drive( location: str, agent_states: List[AgentState], - agent_attributes: List[AgentAttributes], + agent_attributes: Optional[List[AgentAttributes]]=None, + agent_properties: Optional[List[AgentProperties]]=None, recurrent_states: Optional[List[RecurrentState]] = None, traffic_lights_states: Optional[TrafficLightStatesDict] = None, light_recurrent_states: Optional[LightRecurrentStates] = None, @@ -82,6 +84,14 @@ def drive( currently supports 'car' and 'pedestrian'. waypoint: optional [Point], the target waypoint of the agent. + agent_properties: + Agent properties for all agents, replacing soon to be deprecated `agent_attributes`. + List of agent attributes. Each agent requires, length: [float] + width: [float] and rear_axis_offset: [float] all in meters. agent_type: [str], + currently supports 'car' and 'pedestrian'. + waypoint: optional [Point], the target waypoint of the agent. + max_speed: optional [float], the desired maximum speed of the agent in m/s. + recurrent_states: Recurrent states for all agents, obtained from the previous call to :func:`drive` or :func:`initialize`. @@ -153,7 +163,8 @@ def _tolist(input_data: List): model_inputs = dict( location=location, agent_states=[state.tolist() for state in agent_states], - agent_attributes=[state.tolist() for state in agent_attributes], + agent_attributes=[state.tolist() for state in agent_attributes] if agent_attributes is not None else None, + agent_properties=[ap.serialize() for ap in agent_properties] if agent_properties is not None else None, recurrent_states=[r.packed for r in recurrent_states] if recurrent_states is not None else None, traffic_lights_states=traffic_lights_states, light_recurrent_states=[light_recurrent_state.tolist() for light_recurrent_state in light_recurrent_states] @@ -214,7 +225,8 @@ def _tolist(input_data: List): async def async_drive( location: str, agent_states: List[AgentState], - agent_attributes: List[AgentAttributes], + agent_attributes: Optional[List[AgentAttributes]]=None, + agent_properties: Optional[List[AgentProperties]]=None, recurrent_states: Optional[List[RecurrentState]] = None, traffic_lights_states: Optional[TrafficLightStatesDict] = None, light_recurrent_states: Optional[LightRecurrentStates] = None, @@ -239,7 +251,8 @@ def _tolist(input_data: List): model_inputs = dict( location=location, agent_states=[state.tolist() for state in agent_states], - agent_attributes=[state.tolist() for state in agent_attributes], + agent_attributes=[state.tolist() for state in agent_attributes] if agent_attributes is not None else None, + agent_properties=[ap.serialize() for ap in agent_properties] if agent_properties is not None else None, recurrent_states=[r.packed for r in recurrent_states] if recurrent_states is not None else None, traffic_lights_states=traffic_lights_states, light_recurrent_states=[light_recurrent_state.tolist() for light_recurrent_state in light_recurrent_states] diff --git a/invertedai/api/initialize.py b/invertedai/api/initialize.py index dd84d24c..5331537d 100644 --- a/invertedai/api/initialize.py +++ b/invertedai/api/initialize.py @@ -8,6 +8,7 @@ from invertedai.error import TryAgain, InvalidInputType, InvalidInput from invertedai.api.mock import ( get_mock_agent_attributes, + get_mock_agent_properties, get_mock_agent_state, get_mock_recurrent_state, get_mock_birdview, @@ -17,6 +18,7 @@ RecurrentState, AgentState, AgentAttributes, + AgentProperties, TrafficLightStatesDict, Image, InfractionIndicators, @@ -37,6 +39,7 @@ class InitializeResponse(BaseModel): agent_attributes: List[ Optional[AgentAttributes] ] #: Static attributes of all initialized agents. + agent_properties: List[AgentProperties] #: Static agent properties of all initialized agents. birdview: Optional[ Image ] #: If `get_birdview` was set, this contains the resulting image. @@ -52,6 +55,7 @@ class InitializeResponse(BaseModel): def initialize( location: str, agent_attributes: Optional[List[AgentAttributes]] = None, + agent_properties: Optional[List[AgentProperties]] = None, states_history: Optional[List[List[AgentState]]] = None, traffic_light_state_history: Optional[ List[TrafficLightStatesDict] @@ -88,6 +92,11 @@ def initialize( Static attributes for all agents. The pre-defined agents should be specified first, followed by the sampled agents. The optional waypoint passed will be ignored for Initialize. + agent_properties: + Agent properties for all agents, replacing soon to be deprecated `agent_attributes`. + The pre-defined agents should be specified first, followed by the sampled agents. + The optional waypoint passed will be ignored for Initialize. + max_speed: optional [float], the desired maximum speed of the agent in m/s. states_history: History of pre-defined agent states - the outer list is over time and the inner over agents, @@ -128,6 +137,8 @@ def initialize( """ if should_use_mock_api(): + if agent_properties is not None: + agent_properties = [get_mock_agent_properties() for _ in range(agent_count)] if agent_attributes is None: assert agent_count is not None agent_attributes = [get_mock_agent_attributes() for _ in range(agent_count)] @@ -140,6 +151,7 @@ def initialize( response = InitializeResponse( agent_states=agent_states, agent_attributes=agent_attributes, + agent_properties=agent_properties, recurrent_states=recurrent_states, birdview=birdview, infractions=infractions, @@ -155,6 +167,8 @@ def initialize( agent_attributes=agent_attributes if agent_attributes is None else [state.tolist() for state in agent_attributes], + agent_properties=agent_properties if agent_properties is None + else [ap.serialize() if ap else None for ap in agent_properties] , traffic_light_state_history=traffic_light_state_history, get_birdview=get_birdview, location_of_interest=location_of_interest, @@ -173,6 +187,9 @@ def initialize( ], agent_attributes=[ AgentAttributes.fromlist(attr) for attr in response["agent_attributes"] + ] if response["agent_attributes"] is not None else [], + agent_properties=[ + AgentProperties.deserialize(ap) for ap in response["agent_properties"] ], recurrent_states=[ RecurrentState.fromval(r) for r in response["recurrent_states"] @@ -208,6 +225,7 @@ def initialize( async def async_initialize( location: str, agent_attributes: Optional[List[AgentAttributes]] = None, + agent_properties: Optional[List[AgentProperties]] = None, states_history: Optional[List[List[AgentState]]] = None, traffic_light_state_history: Optional[ List[TrafficLightStatesDict] @@ -232,6 +250,8 @@ async def async_initialize( agent_attributes=agent_attributes if agent_attributes is None else [state.tolist() for state in agent_attributes], + agent_properties=agent_properties if agent_properties is None + else [ap.serialize() if ap else None for ap in agent_properties] , traffic_light_state_history=traffic_light_state_history, get_birdview=get_birdview, location_of_interest=location_of_interest, @@ -253,6 +273,9 @@ async def async_initialize( agent_attributes=[ AgentAttributes.fromlist(attr) for attr in response["agent_attributes"] ], + agent_properties=[ + AgentProperties.deserialize(ap) for ap in response["agent_properties"] + ], recurrent_states=[ RecurrentState.fromval(r) for r in response["recurrent_states"] ], diff --git a/invertedai/api/mock.py b/invertedai/api/mock.py index a10a8648..d0434707 100644 --- a/invertedai/api/mock.py +++ b/invertedai/api/mock.py @@ -2,6 +2,7 @@ from invertedai.common import ( AgentAttributes, + AgentProperties, AgentState, RecurrentState, InfractionIndicators, @@ -19,6 +20,11 @@ def get_mock_agent_attributes() -> AgentAttributes: return attributes +def get_mock_agent_properties() -> AgentProperties: + properties = AgentProperties(length=5, width=2, rear_axis_offset=1.4) + return properties + + def get_mock_agent_state() -> AgentState: state = AgentState(center=Point(x=0, y=0), orientation=0, speed=0) return state diff --git a/invertedai/common.py b/invertedai/common.py index 0cea27ed..5a37068d 100644 --- a/invertedai/common.py +++ b/invertedai/common.py @@ -202,7 +202,36 @@ def tolist(self): attr_list.append([self.waypoint.x, self.waypoint.y]) return attr_list +class AgentProperties(BaseModel): + """ + Static attributes of the agent, which don't change over the course of a simulation. + We assume every agent is a rectangle obeying a kinematic bicycle model. + + See Also + -------- + AgentState + """ + + length: Optional[float] = None #: Longitudinal extent of the agent, in meters. + width: Optional[float] = None #: Lateral extent of the agent, in meters. + #: Distance from the agent's center to its rear axis in meters. Determines motion constraints. + rear_axis_offset: Optional[float] = None + agent_type: Optional[str] = 'car' #: Valid types are those in `AgentType`, but we use `str` here for extensibility. + waypoint: Optional[Point] = None #: Target waypoint of the agent. If provided the agent will attempt to reach it. + max_speed: Optional[float] = None #: Maximum speed of the agent in m/s. + @classmethod + def deserialize(cls, val): + return cls(length=val['length'], width=val['width'], rear_axis_offset=val['rear_axis_offset'], agent_type=val['agent_type'], + waypoint=Point(x=val['waypoint'][0], y=val['waypoint'][1]) if val['waypoint'] else None, max_speed=val['max_speed']) + + def serialize(self): + """ + Convert AgentProperties to a valid request format in json + """ + return {"length": self.length, "width": self.width, "rear_axis_offset": self.rear_axis_offset, "agent_type": self.agent_type, + "waypoint": [self.waypoint.x, self.waypoint.y] if self.waypoint else None, "max_speed": self.max_speed} + class AgentState(BaseModel): """ The current or predicted state of a given agent at a given point. diff --git a/tests/test_drive.py b/tests/test_drive.py index 8ae3ba01..8852f1f9 100644 --- a/tests/test_drive.py +++ b/tests/test_drive.py @@ -6,7 +6,7 @@ from invertedai.api.drive import drive, DriveResponse from invertedai.api.location import location_info from invertedai.api.light import light -from invertedai.common import Point +from invertedai.common import Point, AgentProperties from invertedai.error import InvalidRequestError @@ -16,7 +16,7 @@ def recurrent_states_helper(states_to_extend): return result -positive_tests = [ +positive_tests_old = [ ("carla:Town04", None, None, @@ -92,7 +92,7 @@ def recurrent_states_helper(states_to_extend): ] # Notice parameterization in direct drive flows are different -negative_tests = [ +negative_tests_old = [ ("carla:Town03", [dict(center=dict(x=-21.2, y=-17.11), orientation=4.54, speed=1.8), dict(center=dict(x=-5.81, y=-49.47), orientation=1.62, speed=11.4)], @@ -126,7 +126,7 @@ def recurrent_states_helper(states_to_extend): ] -def run_initialize_drive_flow(location, states_history, agent_attributes, get_infractions, agent_count, +def run_initialize_drive_flow(location, states_history, agent_attributes, agent_properties, get_infractions, agent_count, simulation_length: int = 20): location_info_response = location_info(location=location, rendering_fov=200) scene_has_lights = any(actor.agent_type == "traffic_light" for actor in location_info_response.static_actors) @@ -134,17 +134,20 @@ def run_initialize_drive_flow(location, states_history, agent_attributes, get_in initialize_response = initialize( location, agent_attributes=agent_attributes, + agent_properties=agent_properties, states_history=states_history, traffic_light_state_history=None, get_birdview=False, get_infractions=get_infractions, agent_count=agent_count, ) - agent_attributes = initialize_response.agent_attributes + agent_attributes = initialize_response.agent_attributes if agent_attributes is None else None + agent_properties = initialize_response.agent_properties updated_state = initialize_response for t in range(simulation_length): updated_state = drive( agent_attributes=agent_attributes, + agent_properties=agent_properties, agent_states=updated_state.agent_states, recurrent_states=updated_state.recurrent_states, light_recurrent_states=updated_state.light_recurrent_states if scene_has_lights else None, @@ -158,9 +161,10 @@ def run_initialize_drive_flow(location, states_history, agent_attributes, get_in assert updated_state.traffic_lights_states is not None assert updated_state.light_recurrent_states is not None -def run_direct_drive(location, agent_states, agent_attributes, recurrent_states, get_infractions): +def run_direct_drive(location, agent_states, agent_attributes, agent_properties, recurrent_states, get_infractions): drive_response = drive( agent_attributes=agent_attributes, + agent_properties=agent_properties, agent_states=agent_states, recurrent_states=recurrent_states, traffic_lights_states=None, @@ -172,14 +176,137 @@ def run_direct_drive(location, agent_states, agent_attributes, recurrent_states, DriveResponse) and drive_response.agent_states is not None and drive_response.recurrent_states is not None -@pytest.mark.parametrize("location, agent_states, agent_attributes, recurrent_states, get_infractions", negative_tests) -def test_negative(location, agent_states, agent_attributes, recurrent_states, get_infractions): +@pytest.mark.parametrize("location, agent_states, agent_attributes, recurrent_states, get_infractions", negative_tests_old) +def test_negative_old(location, agent_states, agent_attributes, recurrent_states, get_infractions): + with pytest.raises(InvalidRequestError): + run_direct_drive(location, agent_states, agent_attributes, None, recurrent_states, get_infractions) + + +@pytest.mark.parametrize("location, states_history, agent_attributes, get_infractions, agent_count", positive_tests_old) +def test_positive_old(location, states_history, agent_attributes, get_infractions, agent_count, + simulation_length: int = 20): + run_initialize_drive_flow(location, states_history, agent_attributes, None, get_infractions, agent_count, + simulation_length) + + +positive_tests = [ + ("carla:Town04", + None, + None, + False, 5), + ("canada:drake_street_and_pacific_blvd", + None, + [AgentProperties(agent_type='car'), + AgentProperties(), + AgentProperties(agent_type='car'), + AgentProperties()], + False, 5), + ("canada:drake_street_and_pacific_blvd", + None, + [AgentProperties(agent_type='car'), + AgentProperties(), + AgentProperties(agent_type='pedestrian'), + AgentProperties()], + False, 5), + ("canada:drake_street_and_pacific_blvd", + None, + [AgentProperties(agent_type='pedestrian'), + AgentProperties(agent_type='pedestrian'), + AgentProperties(agent_type='pedestrian'), + AgentProperties(agent_type='pedestrian')], + False, 5), + ("canada:ubc_roundabout", + [[dict(center=dict(x=60.82, y=1.22), orientation=0.63, speed=11.43), + dict(center=dict(x=-36.88, y=-33.93), orientation=-2.64, speed=9.43)]], + [AgentProperties(length=5.3, width=2.25, rear_axis_offset=2.01, agent_type='car'), + AgentProperties(length=6.29, width=2.56, rear_axis_offset=2.39, agent_type='car'), + AgentProperties(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=60.82, y=1.22), orientation=0.63, speed=11.43), + dict(center=dict(x=-36.88, y=-33.93), orientation=-2.64, speed=9.43)], + [dict(center=dict(x=62.82, y=3.22), orientation=0.63, speed=11.43), + dict(center=dict(x=-34.88, y=-31.93), orientation=-2.64, speed=9.43)]], + [AgentProperties(length=5.3, width=2.25, rear_axis_offset=2.01, agent_type='car'), + AgentProperties(length=6.29, width=2.56, rear_axis_offset=2.39, agent_type='car'), + AgentProperties(agent_type='car'), + AgentProperties(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=60.82, y=1.22), orientation=0.63, speed=11.43,), + dict(center=dict(x=-36.88, y=-33.93), orientation=-2.64, speed=9.43)], + [dict(center=dict(x=62.82, y=3.22), orientation=0.63, speed=11.43), + dict(center=dict(x=-34.88, y=-31.93), orientation=-2.64, speed=9.43)]], + [AgentProperties(length=5.3, width=2.25, rear_axis_offset=2.01, agent_type='car', waypoint=Point(x=1, y=2)), + AgentProperties(length=6.29, width=2.56, rear_axis_offset=2.39, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + AgentProperties(agent_type='car'), + AgentProperties(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=60.82, y=1.22), orientation=0.63, speed=11.43,), + dict(center=dict(x=-36.88, y=-33.93), orientation=-2.64, speed=9.43)], + [dict(center=dict(x=62.82, y=3.22), orientation=0.63, speed=11.43), + dict(center=dict(x=-34.88, y=-31.93), orientation=-2.64, speed=9.43)]], + [AgentProperties(length=5.3, width=2.25, rear_axis_offset=2.01, waypoint=Point(x=1, y=2)), + AgentProperties(length=6.29, width=2.56, rear_axis_offset=2.39, waypoint=Point(x=1, y=2)), + AgentProperties(agent_type='car'), + AgentProperties(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=60.82, y=1.22), orientation=0.63, speed=11.43,), + dict(center=dict(x=-36.88, y=-33.93), orientation=-2.64, speed=9.43)], + [dict(center=dict(x=62.82, y=3.22), orientation=0.63, speed=11.43), + dict(center=dict(x=-34.88, y=-31.93), orientation=-2.64, speed=9.43)]], + [AgentProperties(length=5.3, width=2.25, rear_axis_offset=2.01, agent_type='pedestrian',waypoint=Point(x=1, y=2)), + AgentProperties(length=6.29, width=2.56, rear_axis_offset=None, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + AgentProperties(agent_type='car'), + AgentProperties(agent_type='car')], + False, None) +] + +# Notice parameterization in direct drive flows are different +negative_tests = [ + ("carla:Town03", + [dict(center=dict(x=-21.2, y=-17.11), orientation=4.54, speed=1.8), + dict(center=dict(x=-5.81, y=-49.47), orientation=1.62, speed=11.4)], + [AgentProperties(length=0.97, agent_type="pedestrian"), + AgentProperties(length=4.86, width=2.12, rear_axis_offset=1.85, agent_type='car')], + [dict(packed=recurrent_states_helper( + [21.203039169311523, -17.10862159729004, -1.3971855640411377, 1.7982733249664307])), + dict(packed=recurrent_states_helper( + [5.810295104980469, -49.47068786621094, 1.5232856273651123, 11.404326438903809]))], + False), + ("carla:Town03", + [dict(center=dict(x=-21.2, y=-17.11), orientation=4.54, speed=1.8), + dict(center=dict(x=-5.81, y=-49.47), orientation=1.62, speed=11.4)], + [AgentProperties(length=0.97, width=1.06, rear_axis_offset=None, agent_type="pedestrian"), + AgentProperties(length=4.86, width=2.12, agent_type='car')], + [dict(packed=recurrent_states_helper( + [21.203039169311523, -17.10862159729004, -1.3971855640411377, 1.7982733249664307])), + dict(packed=recurrent_states_helper( + [5.810295104980469, -49.47068786621094, 1.5232856273651123, 11.404326438903809]))], + False), + ("carla:Town03", + [dict(center=dict(x=-21.2, y=-17.11), orientation=4.54, speed=1.8), + dict(center=dict(x=-5.81, y=-49.47), orientation=1.62, speed=11.4)], + [AgentProperties(length=0.97, width=1.06, agent_type="pedestrian"), + AgentProperties(width=2.12, rear_axis_offset=1.85)], + [dict(packed=recurrent_states_helper( + [21.203039169311523, -17.10862159729004, -1.3971855640411377, 1.7982733249664307])), + dict(packed=recurrent_states_helper( + [5.810295104980469, -49.47068786621094, 1.5232856273651123, 11.404326438903809]))], + False), +] + + +@pytest.mark.parametrize("location, agent_states, agent_properties, recurrent_states, get_infractions", negative_tests) +def test_negative(location, agent_states, agent_properties, recurrent_states, get_infractions): with pytest.raises(InvalidRequestError): - run_direct_drive(location, agent_states, agent_attributes, recurrent_states, get_infractions) + run_direct_drive(location, agent_states, None, agent_properties, recurrent_states, get_infractions) -@pytest.mark.parametrize("location, states_history, agent_attributes, get_infractions, agent_count", positive_tests) -def test_postivie(location, states_history, agent_attributes, get_infractions, agent_count, +@pytest.mark.parametrize("location, states_history, agent_properties, get_infractions, agent_count", positive_tests) +def test_positive(location, states_history, agent_properties, get_infractions, agent_count, simulation_length: int = 20): - run_initialize_drive_flow(location, states_history, agent_attributes, get_infractions, agent_count, + run_initialize_drive_flow(location, states_history, None, agent_properties, get_infractions, agent_count, simulation_length) diff --git a/tests/test_initialize.py b/tests/test_initialize.py index 4265e6aa..f92b3e6c 100644 --- a/tests/test_initialize.py +++ b/tests/test_initialize.py @@ -2,13 +2,13 @@ import pytest sys.path.insert(0, "../../") -from invertedai.common import Point +from invertedai.common import Point, AgentProperties from invertedai.api.initialize import initialize, InitializeResponse from invertedai.api.location import location_info from invertedai.api.light import light from invertedai.error import InvalidRequestError -positive_tests = [ +positive_tests_old = [ ("canada:ubc_roundabout", [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)]], @@ -125,19 +125,10 @@ dict(length=4.55, width=1.94, rear_axis_offset=1.4, agent_type='car', waypoint=Point(x=1, y=2)), dict(agent_type='pedestrian'), dict(waypoint=Point(x=1, y=2))], - False, 6), - ("carla:Town04", - None, - None, - False, 5), + False, 6) ] -negative_tests = [ - ("canada:ubc_roundabout", - [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), - dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)]], - None, - False, None), +negative_tests_old = [ ("canada:ubc_roundabout", [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], @@ -180,10 +171,11 @@ False, None), ] -def run_initialize(location, states_history, agent_attributes, get_infractions, agent_count): +def run_initialize(location, states_history, agent_attributes, agent_properties, get_infractions, agent_count): response = initialize( location, agent_attributes=agent_attributes, + agent_properties=agent_properties, states_history=states_history, traffic_light_state_history=None, get_birdview=False, @@ -196,12 +188,195 @@ def run_initialize(location, states_history, agent_attributes, get_infractions, assert response.light_recurrent_states is not None -@pytest.mark.parametrize("location, states_history, agent_attributes, get_infractions, agent_count", negative_tests) -def test_negative(location, states_history, agent_attributes, get_infractions, agent_count): +@pytest.mark.parametrize("location, states_history, agent_attributes, get_infractions, agent_count", negative_tests_old) +def test_negative_old(location, states_history, agent_attributes, get_infractions, agent_count): + with pytest.raises(InvalidRequestError): + run_initialize(location, states_history, agent_attributes, None, get_infractions, agent_count) + + +@pytest.mark.parametrize("location, states_history, agent_attributes, get_infractions, agent_count", positive_tests_old) +def test_positive_old(location, states_history, agent_attributes, get_infractions, agent_count): + run_initialize(location, states_history, agent_attributes, None, get_infractions, agent_count) + + +positive_tests = [ + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)]], + [AgentProperties(length=1.39, width=1.78, rear_axis_offset=0.0, agent_type='pedestrian'), + AgentProperties(length=1.37, width=1.98, rear_axis_offset=0.0, agent_type='pedestrian'), + AgentProperties(agent_type='pedestrian')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [AgentProperties(length=1.39, width=1.78, agent_type='pedestrian'), + AgentProperties(length=1.37, width=1.98, rear_axis_offset=0.0, agent_type='pedestrian'), + AgentProperties(agent_type='pedestrian'), + AgentProperties(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [AgentProperties(length=1.39, width=1.78, rear_axis_offset=0.0, agent_type='pedestrian'), + AgentProperties(length=1.37, width=1.98, agent_type='pedestrian'), + AgentProperties(agent_type='car'), + AgentProperties()], + False, None), + ("carla:Town03", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [AgentProperties(length=1.39, width=1.78, agent_type='pedestrian'), + AgentProperties(length=1.37, width=1.98, rear_axis_offset=None, agent_type='pedestrian'), + AgentProperties(agent_type='car'), + AgentProperties()], + False, 5), + ("carla:Town03", + None, + [AgentProperties(agent_type='pedestrian'), + AgentProperties(), + AgentProperties(agent_type='car'), + AgentProperties()], + False, 5), + ("carla:Town03", + None, + [AgentProperties(agent_type='pedestrian'), + AgentProperties(), + AgentProperties(agent_type='car'), + AgentProperties()], + False, None), + ("canada:drake_street_and_pacific_blvd", + None, + [AgentProperties(agent_type='pedestrian'), + AgentProperties(), + AgentProperties(agent_type='car'), + AgentProperties()], + False, None), + ("canada:drake_street_and_pacific_blvd", + None, + [AgentProperties(agent_type='pedestrian'), + AgentProperties(), + AgentProperties(agent_type='car'), + AgentProperties()], + False, 5), + ("canada:drake_street_and_pacific_blvd", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [AgentProperties(length=1.39, width=1.78, agent_type='pedestrian'), + AgentProperties(length=1.37, width=1.98, agent_type='pedestrian'), + AgentProperties(agent_type='pedestrian'), + AgentProperties(agent_type='car')], + False, 6), + ("canada:drake_street_and_pacific_blvd", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=17.9, y=7.98), orientation=-2.74, speed=0.06), + dict(center=dict(x=-41.5, y=-34.3), orientation=0.42, speed=0.05)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=17.9, y=7.98), orientation=-2.74, speed=0.06), + dict(center=dict(x=-41.5, y=-34.3), orientation=0.42, speed=0.05)]], + [AgentProperties(length=1.39, width=1.78, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + AgentProperties(length=1.37, width=1.98, rear_axis_offset=None, agent_type="pedestrian", waypoint=Point(x=1, y=2)), + AgentProperties(length=4.55, width=1.94, rear_axis_offset=1.4, agent_type='car', waypoint=Point(x=1, y=2)), + AgentProperties(agent_type='pedestrian'), + AgentProperties(agent_type='car')], + False, 6), + ("canada:drake_street_and_pacific_blvd", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=17.9, y=7.98), orientation=-2.74, speed=0.06), + dict(center=dict(x=-41.5, y=-34.3), orientation=0.42, speed=0.05)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=17.9, y=7.98), orientation=-2.74, speed=0.06), + dict(center=dict(x=-41.5, y=-34.3), orientation=0.42, speed=0.05)]], + [AgentProperties(length=1.39, width=1.78, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + AgentProperties(length=1.37, width=1.98, rear_axis_offset=None, agent_type="pedestrian", waypoint=Point(x=1, y=2)), + AgentProperties(length=4.55, width=1.94, rear_axis_offset=1.4, agent_type='car', waypoint=Point(x=1, y=2)), + AgentProperties(agent_type='pedestrian'), + AgentProperties(waypoint=Point(x=1, y=2))], + False, 6), + ("canada:drake_street_and_pacific_blvd", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=17.9, y=7.98), orientation=-2.74, speed=0.06), + dict(center=dict(x=-41.5, y=-34.3), orientation=0.42, speed=0.05)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=17.9, y=7.98), orientation=-2.74, speed=0.06), + dict(center=dict(x=-41.5, y=-34.3), orientation=0.42, speed=0.05)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=17.9, y=7.98), orientation=-2.74, speed=0.06), + dict(center=dict(x=-41.5, y=-34.3), orientation=0.42, speed=0.05)]], + [AgentProperties(length=1.39, width=1.78, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + AgentProperties(length=1.37, width=1.98, rear_axis_offset=None, agent_type="pedestrian", waypoint=Point(x=1, y=2)), + AgentProperties(length=4.55, width=1.94, rear_axis_offset=1.4, agent_type='car', waypoint=Point(x=1, y=2)), + AgentProperties(agent_type='pedestrian'), + AgentProperties(waypoint=Point(x=1, y=2))], + False, 6), + ("carla:Town04", + None, + None, + False, 5), +] + +negative_tests = [ + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)]], + None, + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [dict(length=1.39, agent_type='pedestrian'), + dict(width=1.98, rear_axis_offset=0.0, agent_type='pedestrian'), + dict(agent_type='pedestrian'), + dict(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [dict(length=1.39, width=1.20, agent_type='pedestrian'), + dict(length=1.37, width=1.98, agent_type='car'), + dict(agent_type='pedestrian'), + dict(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [dict(length=1.39, width=1.26, rear_axis_offset=0.0, agent_type='pedestrian'), + dict(length=1.37, width=1.98, rear_axis_offset=0.0, agent_type='pedestrian'), + dict(width=1.15, agent_type='pedestrian'), + dict(agent_type='car')], + False, None), + ("canada:ubc_roundabout", + [[dict(center=dict(x=-31.1, y=-24.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-46.62, y=-25.02), orientation=0.04, speed=1.09)], + [dict(center=dict(x=-31.1, y=-23.36), orientation=2.21, speed=0.11), + dict(center=dict(x=-47.62, y=-23.02), orientation=0.04, speed=1.09)]], + [dict(length=1.39, width=1.26, rear_axis_offset=0.0, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + dict(length=1.37, width=1.98, rear_axis_offset=0.0, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + dict(width=1.15, agent_type='pedestrian', waypoint=Point(x=1, y=2)), + dict(agent_type='car')], + False, None), +] + +@pytest.mark.parametrize("location, states_history, agent_properties, get_infractions, agent_count", negative_tests) +def test_negative(location, states_history, agent_properties, get_infractions, agent_count): with pytest.raises(InvalidRequestError): - run_initialize(location, states_history, agent_attributes, get_infractions, agent_count) + run_initialize(location, states_history, None, agent_properties, get_infractions, agent_count) -@pytest.mark.parametrize("location, states_history, agent_attributes, get_infractions, agent_count", positive_tests) -def test_positive(location, states_history, agent_attributes, get_infractions, agent_count): - run_initialize(location, states_history, agent_attributes, get_infractions, agent_count) +@pytest.mark.parametrize("location, states_history, agent_properties, get_infractions, agent_count", positive_tests) +def test_positive(location, states_history, agent_properties, get_infractions, agent_count): + run_initialize(location, states_history, None, agent_properties, get_infractions, agent_count) From ccb7987a8c779dedf50df8c61bbcde794a994905 Mon Sep 17 00:00:00 2001 From: rlyu Date: Thu, 2 May 2024 16:40:48 -0700 Subject: [PATCH 2/4] Update max_speed desc --- invertedai/common.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/invertedai/common.py b/invertedai/common.py index 5a37068d..7c23fbaf 100644 --- a/invertedai/common.py +++ b/invertedai/common.py @@ -218,7 +218,7 @@ class AgentProperties(BaseModel): rear_axis_offset: Optional[float] = None agent_type: Optional[str] = 'car' #: Valid types are those in `AgentType`, but we use `str` here for extensibility. waypoint: Optional[Point] = None #: Target waypoint of the agent. If provided the agent will attempt to reach it. - max_speed: Optional[float] = None #: Maximum speed of the agent in m/s. + max_speed: Optional[float] = None #: Maximum speed limit of the agent in m/s. @classmethod def deserialize(cls, val): From 9a89f9e0189db54148919edc13f85688821b14de Mon Sep 17 00:00:00 2001 From: rlyu Date: Thu, 2 May 2024 16:42:22 -0700 Subject: [PATCH 3/4] Update cpp to support agent_properties --- invertedai_cpp/invertedai/data_utils.h | 59 ++++++++++ invertedai_cpp/invertedai/drive_request.cc | 105 ++++++++++++++++-- invertedai_cpp/invertedai/drive_request.h | 13 ++- .../invertedai/initialize_request.cc | 89 +++++++++++++-- .../invertedai/initialize_request.h | 16 ++- .../invertedai/initialize_response.cc | 81 ++++++++++++-- .../invertedai/initialize_response.h | 13 ++- 7 files changed, 339 insertions(+), 37 deletions(-) diff --git a/invertedai_cpp/invertedai/data_utils.h b/invertedai_cpp/invertedai/data_utils.h index 627feeea..6b89a6aa 100644 --- a/invertedai_cpp/invertedai/data_utils.h +++ b/invertedai_cpp/invertedai/data_utils.h @@ -229,6 +229,65 @@ struct AgentAttributes { } }; +/** + * Static agent properties of the agent, which don’t change over the course of a + * simulation. We assume every agent is a rectangle obeying a kinematic bicycle + * model. + * This struct is replacing AgentAttributes. + */ +struct AgentProperties { + /** + * Longitudinal, lateral extent of the agent in meters. + */ + std::optional length, width; + /** + * Distance from the agent’s center to its rear axis in meters. Determines + * motion constraints. + */ + std::optional rear_axis_offset; + /** + * Agent types are used to indicate how that agent might behave in a scenario. + * Currently "car" and "pedestrian" are supported. + */ + std::optional agent_type; + /** + * Target waypoint of the agent. If provided the agent will attempt to reach it. + */ + std::optional waypoint; + /** + * Target waypoint of the agent. If provided the agent will attempt to reach it. + */ + std::optional max_speed; + /** + * Maximum speed limit of the agent in m/s. + */ + + + void printFields() const { + std::cout << "checking fields of current agent..." << std::endl; + if (length.has_value()) { + std::cout << "Length: " << length.value() << std::endl; + } + if (width.has_value()) { + std::cout << "Width: " << width.value() << std::endl; + } + if (rear_axis_offset.has_value()) { + std::cout << "rear_axis_offset: " << rear_axis_offset.value() << std::endl; + } + if (agent_type.has_value()) { + std::cout << "Agent type: " << agent_type.value() << std::endl; + } + if (waypoint.has_value()) { + std::cout << "Waypoint: (" << waypoint.value().x << "," << waypoint.value().y << ")"<< std::endl; + } + if (max_speed.has_value()) { + std::cout << "Max speed: " << max_speed.value() << std::endl; + } + } + + +}; + /** * Dynamic state of a traffic light. */ diff --git a/invertedai_cpp/invertedai/drive_request.cc b/invertedai_cpp/invertedai/drive_request.cc index 081a19d7..955dca52 100644 --- a/invertedai_cpp/invertedai/drive_request.cc +++ b/invertedai_cpp/invertedai/drive_request.cc @@ -20,10 +20,41 @@ DriveRequest::DriveRequest(const std::string &body_str) { }; this->agent_states_.push_back(agent_state); } - this->agent_attributes_.clear(); - for (const auto &element : this->body_json_["agent_attributes"]) { - AgentAttributes agent_attribute(element); - this->agent_attributes_.push_back(agent_attribute); + if (this->body_json_["agent_attributes"].is_null()) { + this->agent_attributes_ = std::nullopt; + } else { + this->agent_attributes_ = std::vector(); + for (const auto &element : this->body_json_["agent_attributes"]) { + AgentAttributes agent_attribute(element); + this->agent_attributes_.value().push_back(agent_attribute); + } + } + if (this->body_json_["agent_properties"].is_null()) { + this->agent_properties_ = std::nullopt; + } else { + this->agent_properties_ = std::vector(); + for (const auto &element : this->body_json_["agent_properties"]) { + AgentProperties ap; + if (element.contains("length")) { + ap.length = element["length"]; + } + if (element.contains("width")) { + ap.width = element["width"]; + } + if (element.contains("rear_axis_offset")) { + ap.rear_axis_offset = element["rear_axis_offset"]; + } + if (element.contains("agent_type")) { + ap.agent_type = element["agent_type"]; + } + if (element.contains("waypoint")) { + ap.waypoint = {element["waypoint"] [0], element["waypoint"] [1]}; + } + if (element.contains("max_speed")) { + ap.max_speed = element["max_speed"]; + } + this->agent_properties_.value().push_back(ap); + } } this->recurrent_states_.clear(); for (const auto &element : this->body_json_["recurrent_states"]) { @@ -94,10 +125,46 @@ void DriveRequest::refresh_body_json_() { }; this->body_json_["agent_states"].push_back(element); } - this->body_json_["agent_attributes"].clear(); - for (const AgentAttributes &agent_attribute : this->agent_attributes_) { - json element = agent_attribute.toJson(); - this->body_json_["agent_attributes"].push_back(element); + if (this->agent_attributes_.has_value()) { + this->body_json_["agent_attributes"].clear(); + for (const AgentAttributes &agent_attribute : this->agent_attributes_.value()) { + json element = agent_attribute.toJson(); + this->body_json_["agent_attributes"].push_back(element); + } + } else { + this->body_json_["agent_attributes"] = nullptr; + } + + if (this->agent_properties_.has_value()) { + this->body_json_["agent_properties"].clear(); + for (const AgentProperties &ap : this->agent_properties_.value()) { + json element; + if (ap.length.has_value()) { + element["length"] = ap.length.value(); + } + + if (ap.width.has_value()) { + element["width"] = ap.width.value(); + } + + if (ap.rear_axis_offset.has_value()) { + element["rear_axis_offset"] = ap.rear_axis_offset.value(); + } + + if (ap.agent_type.has_value()) { + element["agent_type"] = ap.agent_type.value(); + } + + if (ap.max_speed.has_value()) { + element["max_speed"] = ap.max_speed.value(); + } + if (ap.waypoint.has_value()) { + element["waypoint"] = {ap.waypoint->x, ap.waypoint->y}; + } + this->body_json_["agent_properties"].push_back(element); + } + } else { + this->body_json_["agent_properties"] = nullptr; } this->body_json_["recurrent_states"].clear(); for (const std::vector &recurrent_state : this->recurrent_states_) { @@ -156,6 +223,7 @@ void DriveRequest::refresh_body_json_() { void DriveRequest::update(const InitializeResponse &init_res) { this->agent_states_ = init_res.agent_states(); this->agent_attributes_ = init_res.agent_attributes(); + this->agent_properties_ = init_res.agent_properties(); this->recurrent_states_ = init_res.recurrent_states(); this->light_recurrent_states_ = init_res.light_recurrent_states(); } @@ -165,9 +233,20 @@ void DriveRequest::update(const DriveResponse &drive_res) { this->recurrent_states_ = drive_res.recurrent_states(); this->light_recurrent_states_ = drive_res.light_recurrent_states(); } - void DriveRequest::update_attribute(int idx, AgentAttributes &agent_attributes) { - this->agent_attributes_[idx] = agent_attributes; + if (this->agent_attributes_.has_value()) { + this->agent_attributes_.value()[idx] = agent_attributes; + } else { + throw std::runtime_error("Agent attributes is not initialized."); + } +} + +void DriveRequest::update_property(int idx, AgentProperties &agent_properties) { + if (this->agent_properties_.has_value()) { + this->agent_properties_.value()[idx] = agent_properties; + } else { + throw std::runtime_error("Agent properties is not initialized."); + } } std::string DriveRequest::body_str() { @@ -183,10 +262,14 @@ std::vector DriveRequest::agent_states() const { return this->agent_states_; } -std::vector DriveRequest::agent_attributes() const { +std::optional> DriveRequest::agent_attributes() const { return this->agent_attributes_; }; +std::optional> DriveRequest::agent_properties() const { + return this->agent_properties_; +}; + std::optional> DriveRequest::traffic_lights_states() const { return this->traffic_lights_states_; }; diff --git a/invertedai_cpp/invertedai/drive_request.h b/invertedai_cpp/invertedai/drive_request.h index eb10e87b..85e68788 100644 --- a/invertedai_cpp/invertedai/drive_request.h +++ b/invertedai_cpp/invertedai/drive_request.h @@ -20,7 +20,8 @@ class DriveRequest { private: std::string location_; std::vector agent_states_; - std::vector agent_attributes_; + std::optional> agent_attributes_; + std::optional> agent_properties_; std::optional> traffic_lights_states_; std::optional> light_recurrent_states_; std::vector> recurrent_states_; @@ -58,6 +59,10 @@ class DriveRequest { * Update the agent attributes of drive request. */ void update_attribute(int idx, AgentAttributes &agent_attributes); + /** + * Update the agent properties of drive request. + */ + void update_property(int idx, AgentProperties &agent_properties); // getters /** * Get location string in IAI format. @@ -74,7 +79,11 @@ class DriveRequest { /** * Get static attributes for all agents. */ - std::vector agent_attributes() const; + std::optional> agent_attributes() const; + /** + * Get static properties for all agents. + */ + std::optional> agent_properties() const; /** * Get the states of traffic lights. */ diff --git a/invertedai_cpp/invertedai/initialize_request.cc b/invertedai_cpp/invertedai/initialize_request.cc index d9d863de..e6b6ced3 100644 --- a/invertedai_cpp/invertedai/initialize_request.cc +++ b/invertedai_cpp/invertedai/initialize_request.cc @@ -21,10 +21,41 @@ InitializeRequest::InitializeRequest(const std::string &body_str) { } this->states_history_.push_back(agent_states); } - this->agent_attributes_.clear(); - for (const auto &element : this->body_json_["agent_attributes"]) { - AgentAttributes agent_attribute(element); - this->agent_attributes_.push_back(agent_attribute); + if (this->body_json_["agent_attributes"].is_null()) { + this->agent_attributes_ = std::nullopt; + } else { + this->agent_attributes_ = std::vector(); + for (const auto &element : this->body_json_["agent_attributes"]) { + AgentAttributes agent_attribute(element); + this->agent_attributes_.value().push_back(agent_attribute); + } + } + if (this->body_json_["agent_properties"].is_null()) { + this->agent_properties_ = std::nullopt; + } else { + this->agent_properties_ = std::vector(); + for (const auto &element : this->body_json_["agent_properties"]) { + AgentProperties ap; + if (element.contains("length")) { + ap.length = element["length"]; + } + if (element.contains("width")) { + ap.width = element["width"]; + } + if (element.contains("rear_axis_offset")) { + ap.rear_axis_offset = element["rear_axis_offset"]; + } + if (element.contains("agent_type")) { + ap.agent_type = element["agent_type"]; + } + if (element.contains("waypoint")) { + ap.waypoint = {element["waypoint"] [0], element["waypoint"] [1]}; + } + if (element.contains("max_speed")) { + ap.max_speed = element["max_speed"]; + } + this->agent_properties_.value().push_back(ap); + } } this->traffic_light_state_history_.clear(); std::vector> traffic_light_states; @@ -73,10 +104,46 @@ void InitializeRequest::refresh_body_json_() { } this->body_json_["states_history"].push_back(elements); } - this->body_json_["agent_attributes"].clear(); - for (const AgentAttributes &agent_attribute : this->agent_attributes_) { - json element = agent_attribute.toJson(); - this->body_json_["agent_attributes"].push_back(element); + if (this->agent_attributes_.has_value()) { + this->body_json_["agent_attributes"].clear(); + for (const AgentAttributes &agent_attribute : this->agent_attributes_.value()) { + json element = agent_attribute.toJson(); + this->body_json_["agent_attributes"].push_back(element); + } + } else { + this->body_json_["agent_attributes"] = nullptr; + } + + if (this->agent_properties_.has_value()) { + this->body_json_["agent_properties"].clear(); + for (const AgentProperties &ap : this->agent_properties_.value()) { + json element; + if (ap.length.has_value()) { + element["length"] = ap.length.value(); + } + + if (ap.width.has_value()) { + element["width"] = ap.width.value(); + } + + if (ap.rear_axis_offset.has_value()) { + element["rear_axis_offset"] = ap.rear_axis_offset.value(); + } + + if (ap.agent_type.has_value()) { + element["agent_type"] = ap.agent_type.value(); + } + + if (ap.max_speed.has_value()) { + element["max_speed"] = ap.max_speed.value(); + } + if (ap.waypoint.has_value()) { + element["waypoint"] = {ap.waypoint->x, ap.waypoint->y}; + } + this->body_json_["agent_properties"].push_back(element); + } + } else { + this->body_json_["agent_properties"] = nullptr; } this->body_json_["traffic_light_state_history"].clear(); for (const std::map &traffic_light_states : this->traffic_light_state_history_) { @@ -127,10 +194,14 @@ std::vector> InitializeRequest::states_history() const { return this->states_history_; } -std::vector InitializeRequest::agent_attributes() const { +std::optional> InitializeRequest::agent_attributes() const { return this->agent_attributes_; } +std::optional> InitializeRequest::agent_properties() const { + return this->agent_properties_; +} + std::vector> InitializeRequest::traffic_light_state_history() const { return this->traffic_light_state_history_; } diff --git a/invertedai_cpp/invertedai/initialize_request.h b/invertedai_cpp/invertedai/initialize_request.h index 607b0236..1f6a81e4 100644 --- a/invertedai_cpp/invertedai/initialize_request.h +++ b/invertedai_cpp/invertedai/initialize_request.h @@ -17,7 +17,8 @@ class InitializeRequest { std::string location_; std::optional num_agents_to_spawn_; std::vector> states_history_; - std::vector agent_attributes_; + std::optional> agent_attributes_; + std::optional> agent_properties_; std::vector> traffic_light_state_history_; std::optional> location_of_interest_; bool get_birdview_; @@ -55,7 +56,11 @@ class InitializeRequest { /** * Get static attributes for all agents. */ - std::vector agent_attributes() const; + std::optional> agent_attributes() const; + /** + * Get static properties for all agents. + */ + std::optional> agent_properties() const; /** * Get history of traffic light states - the list is * over time, in chronological order. @@ -105,8 +110,11 @@ class InitializeRequest { /** * Set static attributes for all agents. */ - void - set_agent_attributes(const std::vector &agent_attributes); + void set_agent_attributes(const std::vector &agent_attributes); + /** + * Set static properties for all agents. + */ + void set_agent_properties(const std::vector &agent_properties); /** * Set history of traffic light states - the list is * over time, in chronological order. Traffic light states should be provided diff --git a/invertedai_cpp/invertedai/initialize_response.cc b/invertedai_cpp/invertedai/initialize_response.cc index 59ef071b..67ea3a18 100644 --- a/invertedai_cpp/invertedai/initialize_response.cc +++ b/invertedai_cpp/invertedai/initialize_response.cc @@ -17,10 +17,38 @@ InitializeResponse::InitializeResponse(const std::string &body_str) { }; this->agent_states_.push_back(agent_state); } - this->agent_attributes_.clear(); - for (const auto &element : body_json_["agent_attributes"]) { - AgentAttributes agent_attribute(element); - this->agent_attributes_.push_back(agent_attribute); + if (this->body_json_["agent_attributes"].is_null()) { + this->agent_attributes_ = std::nullopt; + } else { + this->agent_attributes_ = std::vector(); + for (const auto &element : this->body_json_["agent_attributes"]) { + AgentAttributes agent_attribute(element); + this->agent_attributes_.value().push_back(agent_attribute); + } + } + + this->agent_properties_ = std::vector(); + for (const auto &element : this->body_json_["agent_properties"]) { + AgentProperties ap; + if (element.contains("length")) { + ap.length = element["length"]; + } + if (element.contains("width")) { + ap.width = element["width"]; + } + if (element.contains("rear_axis_offset") && !element["rear_axis_offset"].is_null()) { + ap.rear_axis_offset = element["rear_axis_offset"]; + } + if (element.contains("agent_type")) { + ap.agent_type = element["agent_type"]; + } + if (element.contains("waypoint") && !element["waypoint"].is_null() ) { + ap.waypoint = {element["waypoint"][0], element["waypoint"][1]}; + } + if (element.contains("max_speed") && !element["max_speed"].is_null()) { + ap.max_speed = element["max_speed"]; + } + this->agent_properties_.push_back(ap); } this->recurrent_states_.clear(); for (const auto &element : body_json_["recurrent_states"]) { @@ -87,10 +115,41 @@ void InitializeResponse::refresh_body_json_() { }; this->body_json_["agent_states"].push_back(element); } - this->body_json_["agent_attributes"].clear(); - for (const AgentAttributes &agent_attribute : this->agent_attributes_) { - json element = agent_attribute.toJson(); - this->body_json_["agent_attributes"].push_back(element); + if (this->body_json_["agent_attributes"].is_null()) { + this->agent_attributes_ = std::nullopt; + } else { + this->agent_attributes_ = std::vector(); + for (const auto &element : this->body_json_["agent_attributes"]) { + AgentAttributes agent_attribute(element); + this->agent_attributes_.value().push_back(agent_attribute); + } + } + this->body_json_["agent_properties"].clear(); + for (const AgentProperties &ap : this->agent_properties_) { + json element; + if (ap.length.has_value()) { + element["length"] = ap.length.value(); + } + + if (ap.width.has_value()) { + element["width"] = ap.width.value(); + } + + if (ap.rear_axis_offset.has_value()) { + element["rear_axis_offset"] = ap.rear_axis_offset.value(); + } + + if (ap.agent_type.has_value()) { + element["agent_type"] = ap.agent_type.value(); + } + + if (ap.max_speed.has_value()) { + element["max_speed"] = ap.max_speed.value(); + } + if (ap.waypoint.has_value()) { + element["waypoint"] = {ap.waypoint->x, ap.waypoint->y}; + } + this->body_json_["agent_properties"].push_back(element); } this->body_json_["recurrent_states"].clear(); for (const std::vector &recurrent_state : this->recurrent_states_) { @@ -147,10 +206,14 @@ std::vector InitializeResponse::agent_states() const { return this->agent_states_; } -std::vector InitializeResponse::agent_attributes() const { +std::optional> InitializeResponse::agent_attributes() const { return this->agent_attributes_; } +std::vector InitializeResponse::agent_properties() const { + return this->agent_properties_; +} + std::vector> InitializeResponse::recurrent_states() const { return this->recurrent_states_; } diff --git a/invertedai_cpp/invertedai/initialize_response.h b/invertedai_cpp/invertedai/initialize_response.h index 763da4c7..912d1f59 100644 --- a/invertedai_cpp/invertedai/initialize_response.h +++ b/invertedai_cpp/invertedai/initialize_response.h @@ -15,7 +15,8 @@ namespace invertedai { class InitializeResponse { private: std::vector agent_states_; - std::vector agent_attributes_; + std::optional> agent_attributes_; + std::vector agent_properties_; std::vector> recurrent_states_; std::optional> traffic_lights_states_; std::optional> light_recurrent_states_; @@ -45,7 +46,11 @@ class InitializeResponse { /** * Get static attributes for all agents. */ - std::vector agent_attributes() const; + std::optional> agent_attributes() const; + /** + * Get static properties for all agents. + */ + std::vector agent_properties() const; /** * Get the recurrent states for all agents. */ @@ -86,6 +91,10 @@ class InitializeResponse { */ void set_agent_attributes(const std::vector &agent_attributes); + /** + * Set static properties for all agents. + */ + void set_agent_properties(const std::vector &agent_properties); /** * Set the recurrent states for all agents. */ From 1508cbc5ced2a3d98c9fc1e3a192a8d19be0dc41 Mon Sep 17 00:00:00 2001 From: rlyu Date: Thu, 2 May 2024 16:44:30 -0700 Subject: [PATCH 4/4] Add max speed example --- invertedai_cpp/examples/BUILD | 14 +++ .../examples/drive_body_template.json | 29 ++++++ ...initialize_body_max_speed_car_example.json | 14 +++ invertedai_cpp/examples/max_speed_example.cc | 93 +++++++++++++++++++ invertedai_cpp/examples/waypoint_example.cc | 16 ++-- 5 files changed, 159 insertions(+), 7 deletions(-) create mode 100644 invertedai_cpp/examples/drive_body_template.json create mode 100755 invertedai_cpp/examples/initialize_body_max_speed_car_example.json create mode 100644 invertedai_cpp/examples/max_speed_example.cc diff --git a/invertedai_cpp/examples/BUILD b/invertedai_cpp/examples/BUILD index eb61cdf7..517cb18a 100644 --- a/invertedai_cpp/examples/BUILD +++ b/invertedai_cpp/examples/BUILD @@ -89,4 +89,18 @@ cc_binary( "@boost//:beast", "@opencv", ], +) + +cc_binary( + name = "max_speed_example", + srcs = ["max_speed_example.cc"], + data = [ + "drive_body.json", + "initialize_body_max_speed_car_example.json", + ], + deps = [ + "//invertedai:api", + "@boost//:beast", + "@opencv", + ], ) \ No newline at end of file diff --git a/invertedai_cpp/examples/drive_body_template.json b/invertedai_cpp/examples/drive_body_template.json new file mode 100644 index 00000000..1bc2fe68 --- /dev/null +++ b/invertedai_cpp/examples/drive_body_template.json @@ -0,0 +1,29 @@ +{ + "location": "canada:ubc_roundabout", + "agent_states": [ + [ + 33.8, + -26.78, + 1.6, + 6.31 + ], + [ + 4.17, + -31.48, + -0.62, + 7.58 + ] + ], + "agent_properties": [ + {"length": 4.55, "width": 1.94, "agent_type": "car", "waypoint": [270.61, -159.85], "max_speed": 1}, + {"length": 4.55, "width": 1.97, "agent_type": "car", "waypoint": [260, -128]} + ], + "recurrent_states": null, + "get_birdview": true, + "get_infractions": true, + "traffic_lights_states": null, + "light_recurrent_states": null, + "random_seed": null, + "rendering_fov": null, + "rendering_center": null +} diff --git a/invertedai_cpp/examples/initialize_body_max_speed_car_example.json b/invertedai_cpp/examples/initialize_body_max_speed_car_example.json new file mode 100755 index 00000000..2a0c4a83 --- /dev/null +++ b/invertedai_cpp/examples/initialize_body_max_speed_car_example.json @@ -0,0 +1,14 @@ +{ + "location": "foretellix:M80_FTX_de_stuttgart", + "num_agents_to_spawn": 2, + "states_history": [[[373, -234, 2.5, 20], [353, -198, 2.5, 19]]], + "agent_properties": [ + {"length": 4.55, "width": 1.94, "rear_axis_offset": 1.7, "agent_type": "car", "waypoint": [270.61, -159.85], "max_speed": 1}, + {"length": 4.55, "width": 1.97, "rear_axis_offset": 1.7, "agent_type": "car", "waypoint": [260, -128]} + ], + "traffic_light_state_history": null, + "get_birdview": true, + "get_infractions": true, + "random_seed": null, + "location_of_interest": null +} diff --git a/invertedai_cpp/examples/max_speed_example.cc b/invertedai_cpp/examples/max_speed_example.cc new file mode 100644 index 00000000..bb32db45 --- /dev/null +++ b/invertedai_cpp/examples/max_speed_example.cc @@ -0,0 +1,93 @@ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../invertedai/api.h" +#include "../invertedai/data_utils.h" + +using AgentAttributes = invertedai::AgentAttributes; +using AgentProperties = invertedai::AgentProperties; + +using Point2d = invertedai::Point2d; + + +int processScenario(const char *bodyPath, const std::string api_key, int timestep); + +int main(int argc, char **argv) { + if (argc != 3) { + std::cerr << "Usage: " << argv[0] << " \n"; + return EXIT_FAILURE; + } + + int timestep; + try { + timestep = std::stoi(argv[1]); + } catch (const std::exception& e) { + std::cerr << "Invalid timestep argument: " << e.what() << std::endl; + return EXIT_FAILURE; + } + + const std::string api_key(argv[2]); + const char* example_body = "examples/initialize_body_max_speed_car_example.json"; + + try { + if (processScenario(example_body, api_key, timestep) != EXIT_SUCCESS) { + return EXIT_FAILURE; + } + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} + +int processScenario(const char *bodyPath, const std::string api_key, int timestep) { + using tcp = net::ip::tcp; // from + using json = nlohmann::json; // from + + net::io_context ioc; + ssl::context ctx(ssl::context::tlsv12_client); + invertedai::Session session(ioc, ctx); + session.set_api_key(api_key); + session.connect(); + + invertedai::InitializeRequest init_req(invertedai::read_file(bodyPath)); + invertedai::InitializeResponse init_res = invertedai::initialize(init_req, &session); + auto image = cv::imdecode(init_res.birdview(), cv::IMREAD_COLOR); + cv::cvtColor(image, image, cv::COLOR_BGR2RGB); + + int frame_width = image.rows; + int frame_height = image.cols; + std::string video_name = "max_speed_example.mp4" ; + + cv::VideoWriter video( + video_name, + cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), + 10, + cv::Size(frame_width, frame_height) + ); + + invertedai::DriveRequest drive_req(invertedai::read_file("examples/drive_body_template.json")); + drive_req.set_location(init_req.location()); + drive_req.update(init_res); + drive_req.set_rendering_center(std::make_pair(313, -194)); // Render the optional birdview in a reasnable area + drive_req.set_rendering_fov(300); + + for (int t = 0; t < timestep; t++) { + invertedai::DriveResponse drive_res = invertedai::drive(drive_req, &session); + auto image = cv::imdecode(drive_res.birdview(), cv::IMREAD_COLOR); + cv::cvtColor(image, image, cv::COLOR_BGR2RGB); + video.write(image); + drive_req.update(drive_res); + std::cout << "Remaining iterations: " << timestep - t << std::endl; + } + + return EXIT_SUCCESS; +} diff --git a/invertedai_cpp/examples/waypoint_example.cc b/invertedai_cpp/examples/waypoint_example.cc index 092b4f29..b2854c38 100644 --- a/invertedai_cpp/examples/waypoint_example.cc +++ b/invertedai_cpp/examples/waypoint_example.cc @@ -77,18 +77,20 @@ int processScenario(const char *bodyPath, const std::string api_key, int timeste drive_req.update(init_res); drive_req.set_rendering_center(std::make_pair(313, -194)); // Render the optional birdview in a reasnable area drive_req.set_rendering_fov(300); - std::vector agent_attributes = drive_req.agent_attributes(); + std::optional> agent_attributes = drive_req.agent_attributes(); for (int t = 0; t < timestep; t++) { invertedai::DriveResponse drive_res = invertedai::drive(drive_req, &session); int agent_idx = 0; - for (AgentAttributes attr : agent_attributes){ - if (attr.waypoint.has_value() && attr.waypoint.value().isCloseTo({drive_res.agent_states()[agent_idx].x, drive_res.agent_states()[agent_idx].y}, 2)) { - attr.waypoint = std::nullopt; - drive_req.update_attribute(agent_idx, attr); - std::cout << "Agent " << agent_idx << " reached waypoint" << std::endl; + if (agent_attributes.has_value()) { + for (AgentAttributes attr : agent_attributes.value()){ + if (attr.waypoint.has_value() && attr.waypoint.value().isCloseTo({drive_res.agent_states()[agent_idx].x, drive_res.agent_states()[agent_idx].y}, 2)) { + attr.waypoint = std::nullopt; + drive_req.update_attribute(agent_idx, attr); + std::cout << "Agent " << agent_idx << " reached waypoint" << std::endl; + } + agent_idx ++; } - agent_idx ++; } auto image = cv::imdecode(drive_res.birdview(), cv::IMREAD_COLOR); cv::cvtColor(image, image, cv::COLOR_BGR2RGB);