Add max speed example

inverted-ai · May 2, 2024 · 1508cbc · 1508cbc
1 parent 9a89f9e
commit 1508cbc
Show file tree

Hide file tree

Showing 5 changed files with 159 additions and 7 deletions.
diff --git 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",
+    ],
 )
diff --git a/invertedai_cpp/examples/drive_body_template.json 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
@@ -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
@@ -0,0 +1,93 @@
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+#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] << " <timestep> <api_key>\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 <boost/asio/ip/tcp.hpp>
+    using json = nlohmann::json; // from <json.hpp>
+
+    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
@@ -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<AgentAttributes> agent_attributes = drive_req.agent_attributes();
+    std::optional<std::vector<AgentAttributes>> 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);