diff --git a/Dockerfile b/Dockerfile index 180acb0..91a9f05 100644 --- a/Dockerfile +++ b/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 && \ diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index a95d463..2f1eb0f 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/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 diff --git a/srunner/autoagents/autoware_nodes/route_node.py b/srunner/autoagents/autoware_nodes/route_node.py index 1824bdf..603494a 100644 --- a/srunner/autoagents/autoware_nodes/route_node.py +++ b/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()