Fix large number of waypoints

Dockerfile

@@ -30,7 +30,7 @@ RUN python3 -m pip install -r requirements.txt && \
     mv docker/PythonAPI.tar ./ && \
     tar -xvf PythonAPI.tar && \
     rm -rf PythonAPI.tar && \
-    mkdir /logs
+    mkdir logs
 
 # update CYCLONE DDS Config for ROS
 RUN mkdir /cyclonedds && \

srunner/autoagents/autoware_agent.py

@@ -141,8 +141,13 @@ def run_step(self) -> None:
                 waypoints.append(
                     self._convert_to_waypoint(waypoint).autoware_from_world_coords()
                 )
+
+            # autoware cannot handle than around 9 waypoints so reduce to 7 waypoints
+            n_waypoints = len(waypoints)
+            segment_size = int(n_waypoints / 7)
+
             # self.route_node.request_route(goal_pose, waypoints)
-            self.route_node.publish_route(goal_pose, waypoints)
+            self.route_node.publish_route(goal_pose, waypoints[0::segment_size])
             self.sent_route = True
 
         # check if the current route is set

srunner/autoagents/autoware_nodes/route_node.py

@@ -98,10 +98,11 @@ def request_route(self, goal: Pose, waypoints: list[Pose]) -> None:
         future.add_done_callback(self.set_route_response_callback)
 
     def publish_route(self, goal: Pose, checkpoints: list[Pose]) -> None:
-        self._publish_goal(goal)
 
         for checkpoint in checkpoints:
             self._publish_checkpoint(checkpoint)
+
+        self._publish_goal(goal)
 
     def _publish_goal(self, goal_point) -> None:
         goal = PoseStamped()