diff --git a/invertedai/api/drive.py b/invertedai/api/drive.py index c7f3cb5..08523c6 100644 --- a/invertedai/api/drive.py +++ b/invertedai/api/drive.py @@ -79,6 +79,7 @@ def drive( 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. recurrent_states: Recurrent states for all agents, obtained from the previous call to diff --git a/invertedai/api/initialize.py b/invertedai/api/initialize.py index 9117b6e..05c4abf 100644 --- a/invertedai/api/initialize.py +++ b/invertedai/api/initialize.py @@ -87,6 +87,7 @@ def initialize( agent_attributes: 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. states_history: History of pre-defined agent states - the outer list is over time and the inner over agents, diff --git a/invertedai/common.py b/invertedai/common.py index 2b023ac..40bdfd1 100644 --- a/invertedai/common.py +++ b/invertedai/common.py @@ -165,19 +165,34 @@ class AgentAttributes(BaseModel): #: 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. @classmethod def fromlist(cls, l): + if len(l) == 5: + length, width, rear_axis_offset, agent_type, waypoint = l + return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=agent_type, waypoints=Point(x=waypoint[0], y=waypoint[1])) if len(l) == 4: - length, width, rear_axis_offset, agent_type = l - return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=agent_type) - elif len(l) == 3: - if type(l[-1]) is not str: - length, width, rear_axis_offset, = l - return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=AgentType.car.value) + if type(l[3]) == list: + if type(l[2]) == str: + length, width, agent_type, waypoint = l + return cls(length=length, width=width, agent_type=agent_type, waypoints=Point(x=waypoint[0], y=waypoint[1])) + else: + length, width, rear_axis_offset, waypoint = l + return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, waypoints=Point(x=waypoint[0], y=waypoint[1])) else: + length, width, rear_axis_offset, agent_type = l + return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=agent_type) + elif len(l) == 3: + if type(l[2]) == list: + length, width, waypoint = l + return cls(length=length, width=width, waypoint=Point(x=waypoint[0], y=waypoint[1])) + elif type(l[2]) == str: length, width, agent_type = l - return cls(length=length, width=width, rear_axis_offset=None, agent_type=agent_type) + return cls(length=length, width=width, agent_type=agent_type) + else: + length, width, rear_axis_offset = l + return cls(length=length, width=width, rear_axis_offset=rear_axis_offset) else: assert len(l) == 1 agent_type, = l @@ -197,6 +212,8 @@ def tolist(self): attr_list.append(self.rear_axis_offset) if self.agent_type is not None: attr_list.append(self.agent_type) + if self.waypoint is not None: + attr_list.append([self.waypoint.x, self.waypoint.y]) return attr_list diff --git a/invertedai_cpp/examples/drive_tests.cc b/invertedai_cpp/examples/drive_tests.cc index fa29673..28613f4 100644 --- a/invertedai_cpp/examples/drive_tests.cc +++ b/invertedai_cpp/examples/drive_tests.cc @@ -9,6 +9,7 @@ #include #include "../invertedai/api.h" +#include "../invertedai/data_utils.h" using tcp = net::ip::tcp; // from using json = nlohmann::json; // from @@ -19,7 +20,8 @@ int main(int argc, char **argv) { { "examples/initialize_body.json", "examples/initialize_with_states_and_attributes.json", - "examples/initialize_sampling_with_types.json" + "examples/initialize_sampling_with_types.json", + "examples/initialize_body_with_multi_agents_waypoint.json" }; try { @@ -40,7 +42,7 @@ int main(int argc, char **argv) { cv::cvtColor(image, image, cv::COLOR_BGR2RGB); int frame_width = image.rows; int frame_height = image.cols; - std::string drive_video_name = "drive_test_" + std::to_string(i) + ".avi"; + std::string drive_video_name = "drive_test_" + std::to_string(i) + ".mp4"; cv::VideoWriter video( drive_video_name, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), @@ -51,7 +53,6 @@ int main(int argc, char **argv) { invertedai::DriveRequest drive_req(invertedai::read_file("examples/drive_body.json")); drive_req.set_location(init_req.location()); drive_req.update(init_res); - for (int t = 0; t < timestep; t++) { // step the simulation by driving the agents invertedai::DriveResponse drive_res = invertedai::drive(drive_req, &session); diff --git a/invertedai_cpp/examples/initialize_body_with_multi_agents_waypoint.json b/invertedai_cpp/examples/initialize_body_with_multi_agents_waypoint.json new file mode 100755 index 0000000..9e9e7b7 --- /dev/null +++ b/invertedai_cpp/examples/initialize_body_with_multi_agents_waypoint.json @@ -0,0 +1,17 @@ +{ + "location": "canada:drake_street_and_pacific_blvd", + "num_agents_to_spawn": 6, + "states_history": [[[-10, -10, 0.5 ,2], [16, 7, -2.5 ,2], [26, -20, 2.3 ,2], [-13, 15, -0.7 ,2], [33, -11, 2.3 ,2]]], + "agent_attributes": [ + [4.55,1.94,1.73,"car", [-9, 15]], + [4.55,1.94,1.73,"car", [18, -20]], + [4.55,1.94,1.73,"car", [-12, 3]], + [4.55,1.94,1.73,"car", [25, 1.5]], + [0.79, 0.78, null, "pedestrian", [12.5, -19.4]] + ], + "traffic_light_state_history": null, + "get_birdview": true, + "get_infractions": true, + "random_seed": null, + "location_of_interest": null +} diff --git a/invertedai_cpp/invertedai/data_utils.h b/invertedai_cpp/invertedai/data_utils.h index 5158332..bbdcd3b 100644 --- a/invertedai_cpp/invertedai/data_utils.h +++ b/invertedai_cpp/invertedai/data_utils.h @@ -64,20 +64,27 @@ struct AgentAttributes { * 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; void printFields() const { std::cout << "checking fields of current agent..." << std::endl; if (length.has_value()) { - std::cout << "Length: " << length.value() << std::endl; + std::cout << "Length: " << length.value() << std::endl; } if (width.has_value()) { - std::cout << "Width: " << width.value() << std::endl; + std::cout << "Width: " << width.value() << std::endl; } if (rear_axis_offset.has_value()) { - std::cout << "rear_axis_offset: " << rear_axis_offset.value() << std::endl; + 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; + std::cout << "Agent type: " << agent_type.value() << std::endl; + } + if (waypoint.has_value()) { + std::cout << "Waypoints: (" << waypoint.value().x << "," << waypoint.value().y << ")"<< std::endl; } } @@ -95,12 +102,20 @@ struct AgentAttributes { if (agent_type.has_value()) { attr_vector.push_back(agent_type.value()); } + std::vector> waypoint_vector; + if (waypoint.has_value()) { + waypoint_vector.push_back({waypoint.value().x, waypoint.value().y}); + } json jsonArray = json::array(); for (const auto &element : attr_vector) { std::visit([&jsonArray](const auto& value) { jsonArray.push_back(value); }, element); } + for (const auto &element : waypoint_vector) { + jsonArray.push_back(element); + } + return jsonArray; } @@ -116,18 +131,62 @@ struct AgentAttributes { else if (element[2].is_number()) { rear_axis_offset = element[2]; } + else if (element[2].is_array()) { + waypoint = {element[2][0], element[2][1]}; + } length = element[0]; width = element[1]; } - else if(size == 4) { + else if (size == 4) + { + length = element[0]; + width = element[1]; + if (element[3].is_array()) + { + waypoint = {element[3][0], element[3][1]}; + if (element[2].is_string()) + { + agent_type = element[2]; + } + else if (element[2].is_number()) + { + rear_axis_offset = element[2]; + } + } + else if (element[3].is_string()){ + { + agent_type = element[3]; + if (element[2].is_number()) + { + rear_axis_offset = element[2]; + } + else + { + rear_axis_offset = 0.0; + } + } + } + else + { + throw std::runtime_error("agent_type must be a string"); + } + + } + else if(size == 5) { length = element[0]; width = element[1]; - if (element[2].is_number()) { - rear_axis_offset = element[2]; - } else { - rear_axis_offset = 0.0; + if (element[2].is_number()) + { + rear_axis_offset = element[2]; } - agent_type = element[3]; + else + { + rear_axis_offset = 0.0; + } + if (element[3].is_string()) { + agent_type = element[3]; + } + waypoint = {element[4][0], element[4][1]}; } } }; diff --git a/invertedai_cpp/invertedai/drive_request.cc b/invertedai_cpp/invertedai/drive_request.cc index a0c4060..081a19d 100644 --- a/invertedai_cpp/invertedai/drive_request.cc +++ b/invertedai_cpp/invertedai/drive_request.cc @@ -166,6 +166,10 @@ void DriveRequest::update(const DriveResponse &drive_res) { this->light_recurrent_states_ = drive_res.light_recurrent_states(); } +void DriveRequest::update_attribute(int idx, AgentAttributes &agent_attributes) { + this->agent_attributes_[idx] = agent_attributes; +} + std::string DriveRequest::body_str() { this->refresh_body_json_(); return this->body_json_.dump(); diff --git a/invertedai_cpp/invertedai/drive_request.h b/invertedai_cpp/invertedai/drive_request.h index 1aefe9e..eb10e87 100644 --- a/invertedai_cpp/invertedai/drive_request.h +++ b/invertedai_cpp/invertedai/drive_request.h @@ -54,7 +54,10 @@ class DriveRequest { * recurrent_states) in the drive response. */ void update(const DriveResponse &drive_res); - + /** + * Update the agent attributes of drive request. + */ + void update_attribute(int idx, AgentAttributes &agent_attributes); // getters /** * Get location string in IAI format. diff --git a/tests/test_drive.py b/tests/test_drive.py index d65f605..7ba17b4 100644 --- a/tests/test_drive.py +++ b/tests/test_drive.py @@ -6,6 +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.error import InvalidRequestError @@ -58,6 +59,36 @@ def recurrent_states_helper(states_to_extend): dict(agent_type='car'), dict(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)]], + [dict(length=5.3, width=2.25, rear_axis_offset=2.01, agent_type='car', waypoints=Point(x=1, y=2)), + dict(length=6.29, width=2.56, rear_axis_offset=2.39, agent_type='pedestrian', waypoints=Point(x=1, y=2)), + dict(agent_type='car'), + dict(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)]], + [dict(length=5.3, width=2.25, rear_axis_offset=2.01, waypoints=Point(x=1, y=2)), + dict(length=6.29, width=2.56, rear_axis_offset=2.39, waypoints=Point(x=1, y=2)), + dict(agent_type='car'), + dict(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)]], + [dict(length=5.3, width=2.25, rear_axis_offset=2.01, agent_type='pedestrian',waypoints=Point(x=1, y=2)), + dict(length=6.29, width=2.56, rear_axis_offset=None, agent_type='pedestrian', waypoints=Point(x=1, y=2)), + dict(agent_type='car'), + dict(agent_type='car')], + False, None) ] # Notice parameterization in direct drive flows are different diff --git a/tests/test_initialize.py b/tests/test_initialize.py index 65fe589..28c3e1d 100644 --- a/tests/test_initialize.py +++ b/tests/test_initialize.py @@ -2,6 +2,7 @@ import pytest sys.path.insert(0, "../../") +from invertedai.common import Point from invertedai.api.initialize import initialize, InitializeResponse from invertedai.api.location import location_info from invertedai.api.light import light @@ -83,6 +84,19 @@ dict(agent_type='pedestrian'), dict(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)]], + [dict(length=1.39, width=1.78, agent_type='pedestrian', waypoints=Point(x=1, y=2)), + dict(length=1.37, width=1.98, rear_axis_offset=None, agent_type="pedestrian", waypoints=Point(x=1, y=2)), + dict(length=4.55, width=1.94, rear_axis_offset=1.4, agent_type='car', waypoints=Point(x=1, y=2)), + dict(agent_type='pedestrian'), + dict(agent_type='car')], + False, 6), ("carla:Town04", None, None, @@ -125,6 +139,16 @@ 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', waypoints=Point(x=1, y=2)), + dict(length=1.37, width=1.98, rear_axis_offset=0.0, agent_type='pedestrian', waypoints=Point(x=1, y=2)), + dict(width=1.15, agent_type='pedestrian', waypoints=Point(x=1, y=2)), + dict(agent_type='car')], + False, None), ] def run_initialize(location, states_history, agent_attributes, get_infractions, agent_count):