From 9fe8d23ca63eb373df80495461a5382a8b78050f Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Wed, 17 Jan 2024 14:47:57 -0800
Subject: [PATCH 01/13] Update python client to include optional waypoints in
 agent attributes

---
 invertedai/common.py | 31 ++++++++++++++++++++++++-------
 1 file changed, 24 insertions(+), 7 deletions(-)

diff --git a/invertedai/common.py b/invertedai/common.py
index c4c7f17f..af3099a9 100644
--- a/invertedai/common.py
+++ b/invertedai/common.py
@@ -151,19 +151,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.
+    waypoints: Optional[Point] = None  #: Target waypoint of the agent.
 
     @classmethod
     def fromlist(cls, l):
+        if len(l) == 5:
+            length, width, rear_axis_offset, agent_type, waypoints = l
+            return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=agent_type, waypoints=waypoints)
         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, waypoints = l
+                    return cls(length=length, width=width, agent_type=agent_type, waypoints=waypoints)
+                else:
+                    length, width, rear_axis_offset, waypoints = l
+                    return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, waypoints=waypoints)
             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, waypoints = l
+                return cls(length=length, width=width, waypoints=waypoints)
+            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
@@ -183,6 +198,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.waypoints is not None:
+            attr_list.append([self.waypoints.x, self.waypoints.y])
         return attr_list
 
 

From ca7cd0981ecbe6a68446b266a8793a69b1b9e543 Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Wed, 17 Jan 2024 14:49:15 -0800
Subject: [PATCH 02/13] Update cpp client for wapoints

---
 invertedai_cpp/invertedai/data_utils.h | 69 ++++++++++++++++++++++----
 1 file changed, 59 insertions(+), 10 deletions(-)

diff --git a/invertedai_cpp/invertedai/data_utils.h b/invertedai_cpp/invertedai/data_utils.h
index 2ca25451..d46be4eb 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<std::string> agent_type;
+  /**
+   * Define target waypoints for each agent.
+   */
+  std::optional<Point2d> waypoints;
 
   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 (waypoints.has_value()) {
+      std::cout << "Waypoints: (" << waypoints.value().x << "," << waypoints.value().y << ")"<< std::endl;
     }
   }
 
@@ -95,6 +102,10 @@ struct AgentAttributes {
     if (agent_type.has_value()) {
         attr_vector.push_back(agent_type.value());
     }
+    if (waypoints.has_value()) {
+        attr_vector.push_back(waypoints.value().x);
+        attr_vector.push_back(waypoints.value().y);
+    }
     json jsonArray = json::array();
     for (const auto &element : attr_vector) {
         std::visit([&jsonArray](const auto& value) {
@@ -116,18 +127,56 @@ struct AgentAttributes {
         else if (element[2].is_number()) {
             rear_axis_offset = element[2];
         }
+        else if (element[2].is_array()) {
+            waypoints = {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())
+      {
+        waypoints = {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;
-        }
+        rear_axis_offset = element[2];
         agent_type = element[3];
+        waypoints = {element[4][0], element[4][1]};
+    }
+    else {
+        std::cout << "Error: invalid agent attributes" << std::endl;
     }
   }
 };

From a7299b7117f0694226cabe94ed87d61b711bedac Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Wed, 17 Jan 2024 17:34:26 -0800
Subject: [PATCH 03/13] Adding python test cases for drive

---
 tests/test_drive.py | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/tests/test_drive.py b/tests/test_drive.py
index 8a138a64..8e462f3b 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

From 69b1931771c5b6f1d0d4916d7c7e2a9fbd9b39ad Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 10:21:20 -0800
Subject: [PATCH 04/13] Update data utils for waypoints

---
 invertedai_cpp/invertedai/data_utils.h | 19 +++++++++++++------
 1 file changed, 13 insertions(+), 6 deletions(-)

diff --git a/invertedai_cpp/invertedai/data_utils.h b/invertedai_cpp/invertedai/data_utils.h
index d46be4eb..96492b3e 100644
--- a/invertedai_cpp/invertedai/data_utils.h
+++ b/invertedai_cpp/invertedai/data_utils.h
@@ -69,6 +69,11 @@ struct AgentAttributes {
    */
   std::optional<Point2d> waypoints;
 
+  void setWaypoints(double x, double y)
+  {
+    waypoints = Point2d{x, y};
+  }
+
   void printFields() const {
     std::cout << "checking fields of current agent..." << std::endl;
     if (length.has_value()) {
@@ -102,9 +107,9 @@ struct AgentAttributes {
     if (agent_type.has_value()) {
         attr_vector.push_back(agent_type.value());
     }
-    if (waypoints.has_value()) {
-        attr_vector.push_back(waypoints.value().x);
-        attr_vector.push_back(waypoints.value().y);
+    std::vector<std::vector<double>> waypoint_vector;
+    if (waypoints.has_value()) {  
+        waypoint_vector.push_back({waypoints.value().x, waypoints.value().y});
     }
     json jsonArray = json::array();
     for (const auto &element : attr_vector) {
@@ -112,6 +117,10 @@ struct AgentAttributes {
             jsonArray.push_back(value);
         }, element);
     }
+    for (const auto &element : waypoint_vector) {
+        jsonArray.push_back(element);
+    }
+
     return jsonArray;
   }
 
@@ -175,9 +184,7 @@ struct AgentAttributes {
         agent_type = element[3];
         waypoints = {element[4][0], element[4][1]};
     }
-    else {
-        std::cout << "Error: invalid agent attributes" << std::endl;
-    }
+  std::cout << "agent attributes: " << toJson() << std::endl;
   }
 };
 

From d3280465444db9813af6ed44774176c18bf9e2ea Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 10:22:25 -0800
Subject: [PATCH 05/13] Adding waypoints to drive tests

---
 invertedai_cpp/examples/drive_tests.cc     | 5 ++++-
 invertedai_cpp/invertedai/drive_request.cc | 4 ++++
 invertedai_cpp/invertedai/drive_request.h  | 5 ++++-
 3 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/invertedai_cpp/examples/drive_tests.cc b/invertedai_cpp/examples/drive_tests.cc
index fa29673b..b047635f 100644
--- a/invertedai_cpp/examples/drive_tests.cc
+++ b/invertedai_cpp/examples/drive_tests.cc
@@ -9,6 +9,7 @@
 #include <opencv2/imgproc.hpp>
 
 #include "../invertedai/api.h"
+#include "../invertedai/data_utils.h"
 
 using tcp = net::ip::tcp;    // from <boost/asio/ip/tcp.hpp>
 using json = nlohmann::json; // from <json.hpp>
@@ -51,7 +52,9 @@ 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);
-
+        auto first_agent_attributes = drive_req.agent_attributes()[0];
+        first_agent_attributes.setWaypoints(0, 0);
+        drive_req.update_attribute(0, first_agent_attributes);
         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/invertedai/drive_request.cc b/invertedai_cpp/invertedai/drive_request.cc
index df42a2fe..5e8426d3 100644
--- a/invertedai_cpp/invertedai/drive_request.cc
+++ b/invertedai_cpp/invertedai/drive_request.cc
@@ -132,6 +132,10 @@ void DriveRequest::update(const DriveResponse &drive_res) {
   this->recurrent_states_ = drive_res.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 21e4045a..ef18088b 100644
--- a/invertedai_cpp/invertedai/drive_request.h
+++ b/invertedai_cpp/invertedai/drive_request.h
@@ -52,7 +52,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.

From dba086a97ffe2b937e045bce8b60e919386d7a90 Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 10:41:07 -0800
Subject: [PATCH 06/13] Adding tests for initialize

---
 tests/test_initialize.py | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/tests/test_initialize.py b/tests/test_initialize.py
index 830e4053..7f222faf 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, 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):

From b7afe6419bffcc2b655b90a4c3a056dbede9bb3b Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 10:59:25 -0800
Subject: [PATCH 07/13] Update doc for waypoints

---
 invertedai/common.py                   | 2 +-
 invertedai_cpp/invertedai/data_utils.h | 3 +--
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/invertedai/common.py b/invertedai/common.py
index af3099a9..081c6297 100644
--- a/invertedai/common.py
+++ b/invertedai/common.py
@@ -151,7 +151,7 @@ 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.
-    waypoints: Optional[Point] = None  #: Target waypoint of the agent.
+    waypoints: Optional[Point] = None  #: Target waypoint of the agent. If provided the agent will attempt to reach it.
 
     @classmethod
     def fromlist(cls, l):
diff --git a/invertedai_cpp/invertedai/data_utils.h b/invertedai_cpp/invertedai/data_utils.h
index 96492b3e..f299f56a 100644
--- a/invertedai_cpp/invertedai/data_utils.h
+++ b/invertedai_cpp/invertedai/data_utils.h
@@ -65,7 +65,7 @@ struct AgentAttributes {
    */
   std::optional<std::string> agent_type;
   /**
-   * Define target waypoints for each agent.
+   *  Target waypoint of the agent. If provided the agent will attempt to reach it.
    */
   std::optional<Point2d> waypoints;
 
@@ -184,7 +184,6 @@ struct AgentAttributes {
         agent_type = element[3];
         waypoints = {element[4][0], element[4][1]};
     }
-  std::cout << "agent attributes: " << toJson() << std::endl;
   }
 };
 

From b1f7f210b8fd3c50ce46b852e762b6491a67c9c9 Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 11:00:08 -0800
Subject: [PATCH 08/13] Default to save in mp4 format in drive tests

---
 invertedai_cpp/examples/drive_tests.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/invertedai_cpp/examples/drive_tests.cc b/invertedai_cpp/examples/drive_tests.cc
index b047635f..b96fea27 100644
--- a/invertedai_cpp/examples/drive_tests.cc
+++ b/invertedai_cpp/examples/drive_tests.cc
@@ -41,7 +41,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'),

From 49945805ab08474c6a06bb20ea5d247e0c0d15f8 Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 15:57:52 -0800
Subject: [PATCH 09/13] Fix parsing logic

---
 invertedai_cpp/invertedai/data_utils.h | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/invertedai_cpp/invertedai/data_utils.h b/invertedai_cpp/invertedai/data_utils.h
index 960cfc23..b439a583 100644
--- a/invertedai_cpp/invertedai/data_utils.h
+++ b/invertedai_cpp/invertedai/data_utils.h
@@ -180,8 +180,17 @@ struct AgentAttributes {
     else if(size == 5) {
         length = element[0];
         width = element[1];
-        rear_axis_offset = element[2];
-        agent_type = element[3];
+        if (element[2].is_number())
+        {
+          rear_axis_offset = element[2];
+        }
+        else
+        {
+          rear_axis_offset = 0.0;
+        }
+        if (element[3].is_string()) {
+            agent_type = element[3];
+        }
         waypoints = {element[4][0], element[4][1]};
     }
   }

From b73efecb9f73b984498c0de41311c25b5f8d391d Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 15:59:58 -0800
Subject: [PATCH 10/13] Adding a miscellaneous cpp drive test cases for multi
 type agents with waypoints with traffic light

---
 invertedai_cpp/examples/drive_tests.cc          |  6 ++----
 ...tialize_body_with_multi_agents_waypoint.json | 17 +++++++++++++++++
 2 files changed, 19 insertions(+), 4 deletions(-)
 create mode 100755 invertedai_cpp/examples/initialize_body_with_multi_agents_waypoint.json

diff --git a/invertedai_cpp/examples/drive_tests.cc b/invertedai_cpp/examples/drive_tests.cc
index b96fea27..28613f47 100644
--- a/invertedai_cpp/examples/drive_tests.cc
+++ b/invertedai_cpp/examples/drive_tests.cc
@@ -20,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 {
@@ -52,9 +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);
-        auto first_agent_attributes = drive_req.agent_attributes()[0];
-        first_agent_attributes.setWaypoints(0, 0);
-        drive_req.update_attribute(0, first_agent_attributes);
         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 00000000..9e9e7b7d
--- /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
+}

From 3bca4c2f076b2becec8ad4b56b0e60e08576f0c8 Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 16:00:19 -0800
Subject: [PATCH 11/13] Update python from list logic

---
 invertedai/common.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/invertedai/common.py b/invertedai/common.py
index eed93157..20b504c5 100644
--- a/invertedai/common.py
+++ b/invertedai/common.py
@@ -171,22 +171,22 @@ class AgentAttributes(BaseModel):
     def fromlist(cls, l):
         if len(l) == 5:
             length, width, rear_axis_offset, agent_type, waypoints = l
-            return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=agent_type, waypoints=waypoints)
+            return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=agent_type, waypoints=Point(x=waypoints[0], y=waypoints[1]))
         if len(l) == 4:
             if type(l[3]) == list:
                 if type(l[2]) == str:
                     length, width, agent_type, waypoints = l
-                    return cls(length=length, width=width, agent_type=agent_type, waypoints=waypoints)
+                    return cls(length=length, width=width, agent_type=agent_type, waypoints=Point(x=waypoints[0], y=waypoints[1]))
                 else:
                     length, width, rear_axis_offset, waypoints = l
-                    return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, waypoints=waypoints)
+                    return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, waypoints=Point(x=waypoints[0], y=waypoints[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, waypoints = l
-                return cls(length=length, width=width, waypoints=waypoints)
+                return cls(length=length, width=width, waypoints=Point(x=waypoints[0], y=waypoints[1]))
             elif type(l[2]) == str:
                 length, width, agent_type = l
                 return cls(length=length, width=width, agent_type=agent_type)

From 55572c2c32e24d7db83975c993311bdd75b097ca Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 16:11:31 -0800
Subject: [PATCH 12/13] Rename symbol to use singular waypoint

---
 invertedai/api/drive.py                |  1 +
 invertedai/api/initialize.py           |  1 +
 invertedai/common.py                   | 22 +++++++++++-----------
 invertedai_cpp/invertedai/data_utils.h | 21 ++++++++-------------
 4 files changed, 21 insertions(+), 24 deletions(-)

diff --git a/invertedai/api/drive.py b/invertedai/api/drive.py
index c7f3cb5b..08523c6b 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 9117b6e0..05c4abfb 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 20b504c5..40bdfd19 100644
--- a/invertedai/common.py
+++ b/invertedai/common.py
@@ -165,28 +165,28 @@ 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.
-    waypoints: Optional[Point] = None  #: Target waypoint of the agent. If provided the agent will attempt to reach it.
+    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, waypoints = l
-            return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, agent_type=agent_type, waypoints=Point(x=waypoints[0], y=waypoints[1]))
+            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:
             if type(l[3]) == list:
                 if type(l[2]) == str:
-                    length, width, agent_type, waypoints = l
-                    return cls(length=length, width=width, agent_type=agent_type, waypoints=Point(x=waypoints[0], y=waypoints[1]))
+                    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, waypoints = l
-                    return cls(length=length, width=width, rear_axis_offset=rear_axis_offset, waypoints=Point(x=waypoints[0], y=waypoints[1]))
+                    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, waypoints = l
-                return cls(length=length, width=width, waypoints=Point(x=waypoints[0], y=waypoints[1]))
+                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, agent_type=agent_type)
@@ -212,8 +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.waypoints is not None:
-            attr_list.append([self.waypoints.x, self.waypoints.y])
+        if self.waypoint is not None:
+            attr_list.append([self.waypoint.x, self.waypoint.y])
         return attr_list
 
 
diff --git a/invertedai_cpp/invertedai/data_utils.h b/invertedai_cpp/invertedai/data_utils.h
index b439a583..bbdcd3b2 100644
--- a/invertedai_cpp/invertedai/data_utils.h
+++ b/invertedai_cpp/invertedai/data_utils.h
@@ -67,12 +67,7 @@ struct AgentAttributes {
   /**
    *  Target waypoint of the agent. If provided the agent will attempt to reach it.
    */
-  std::optional<Point2d> waypoints;
-
-  void setWaypoints(double x, double y)
-  {
-    waypoints = Point2d{x, y};
-  }
+  std::optional<Point2d> waypoint;
 
   void printFields() const {
     std::cout << "checking fields of current agent..." << std::endl;
@@ -88,8 +83,8 @@ struct AgentAttributes {
     if (agent_type.has_value()) {
       std::cout << "Agent type: " << agent_type.value() << std::endl;
     }
-    if (waypoints.has_value()) {
-      std::cout << "Waypoints: (" << waypoints.value().x << "," << waypoints.value().y << ")"<< std::endl;
+    if (waypoint.has_value()) {
+      std::cout << "Waypoints: (" << waypoint.value().x << "," << waypoint.value().y << ")"<< std::endl;
     }
   }
 
@@ -108,8 +103,8 @@ struct AgentAttributes {
         attr_vector.push_back(agent_type.value());
     }
     std::vector<std::vector<double>> waypoint_vector;
-    if (waypoints.has_value()) {  
-        waypoint_vector.push_back({waypoints.value().x, waypoints.value().y});
+    if (waypoint.has_value()) {  
+        waypoint_vector.push_back({waypoint.value().x, waypoint.value().y});
     }
     json jsonArray = json::array();
     for (const auto &element : attr_vector) {
@@ -137,7 +132,7 @@ struct AgentAttributes {
             rear_axis_offset = element[2];
         }
         else if (element[2].is_array()) {
-            waypoints = {element[2][0], element[2][1]};
+            waypoint = {element[2][0], element[2][1]};
         }
         length = element[0];
         width = element[1];
@@ -148,7 +143,7 @@ struct AgentAttributes {
       width = element[1];
       if (element[3].is_array())
       {
-        waypoints = {element[3][0], element[3][1]};
+        waypoint = {element[3][0], element[3][1]};
         if (element[2].is_string())
         {
           agent_type = element[2];
@@ -191,7 +186,7 @@ struct AgentAttributes {
         if (element[3].is_string()) {
             agent_type = element[3];
         }
-        waypoints = {element[4][0], element[4][1]};
+        waypoint = {element[4][0], element[4][1]};
     }
   }
 };

From fa10948ea1ed47774a2118d4b3b4a6e31e511499 Mon Sep 17 00:00:00 2001
From: rlyu <ruishenlyu@gmail.com>
Date: Thu, 18 Jan 2024 16:19:31 -0800
Subject: [PATCH 13/13] Bug fix tests

---
 tests/test_initialize.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/test_initialize.py b/tests/test_initialize.py
index c8a9e46c..28c3e1dd 100644
--- a/tests/test_initialize.py
+++ b/tests/test_initialize.py
@@ -92,7 +92,7 @@
        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, 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')],