From ae8c63cd670ba2348b0ae00b77e93ca83e5171ef Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Mon, 4 Aug 2025 16:35:33 +0100 Subject: [PATCH 1/3] bug fixes --- srunner/autoagents/autoware_agent.py | 2 +- .../autoware_nodes/autoware_types/waypoint.py | 43 ++++--------------- 2 files changed, 9 insertions(+), 36 deletions(-) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 6d83448..cb9240a 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -102,7 +102,7 @@ def _convert_to_waypoint(self, point): point[0].location.x, point[0].location.y, point[0].location.z, - marker_pub=self.autoware_node.marker_publisher, + node=self.autoware_node, ) def destroy(self) -> None: diff --git a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py index a463d7b..835fc3d 100644 --- a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py +++ b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py @@ -1,5 +1,4 @@ import carla -from math import atan2 from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from visualization_msgs.msg import Marker from tf_transformations import quaternion_from_euler @@ -9,12 +8,13 @@ class Waypoint(object): - def __init__(self, x, y, z, marker_pub=None): + def __init__(self, x, y, z, node=None): self.x = x self.y = y self.z = z - self.marker_pub = marker_pub + self.node = node + self.marker_pub = node.marker_pub self.client = CarlaDataProvider.get_client() @@ -31,11 +31,10 @@ def autoware_from_world_coords(self) -> Pose: ros_point.y = -self.y ros_point.z = self.z - orientation = self._get_orientation() - curr_location = carla.Location(self.x, self.y, self.z) orientation = ( self.client.get_world() + .get_map() .get_waypoint( curr_location, project_to_road=True, lane_type=carla.LaneType.Driving ) @@ -54,44 +53,18 @@ def autoware_from_world_coords(self) -> Pose: w=qw, ) + self.pose = pose + if self.marker_pub is not None: print(self) self.marker_pub.publish(self._publish_marker(pose)) - self.pose = pose - return pose - def _get_orientation(self) -> dict: - point1 = ( - self.client.get_world() - .get_map() - .get_waypoint( - curr_location, project_to_road=True, lane_type=carla.LaneType.Driving - ) - ) - - # get next waypoint that is 0.5 meters away - point2 = point1.next(0.5)[0] - - dx = point2.transform.location.x - point1.transform.location.x - dy = point2.transform.location.y - point1.transform.location.y - yaw = atan2(dy, dx) - - # rounding is need by autoware to function properly - qx, qy, qz, qw = quaternion_from_euler(0, 0, round(yaw, 1)) - - return { - "x": qx, - "y": qy, - "z": qz, - "w": qw, - } - def _publish_marker(self, pose): marker = Marker() marker.header.frame_id = "/map" - marker.header.stamp = self.get_clock().now().to_msg() + marker.header.stamp = self.node.get_clock().now().to_msg() marker.type = marker.ARROW marker.action = marker.ADD @@ -100,7 +73,7 @@ def _publish_marker(self, pose): marker.scale.x = 0.5 marker.scale.y = 0.05 - marker.scale.z = 1 + marker.scale.z = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 From 12f452356266e2c820f078c90bafc208a1ca22db Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Tue, 5 Aug 2025 09:52:39 +0100 Subject: [PATCH 2/3] added fix for sending waypoints by converting to radians --- srunner/autoagents/autoware_agent.py | 11 +++++++---- .../autoware_nodes/autoware_types/waypoint.py | 12 ++++++------ 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index cb9240a..e2383dd 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -127,14 +127,17 @@ def run_step(self) -> None: if self.autoware_state.is_ready_publish_route() and self.agent_set_route: waypoints = [] - goal_pose = self._convert_to_waypoint( - self.goal_pose_world - ).autoware_from_world_coords() + goal_pose = self._convert_to_waypoint(self.goal_pose_world) + + goal_pose._publish_marker(goal_pose.autoware_from_world_coords()) + for waypoint in self.waypoints_world: waypoints.append( self._convert_to_waypoint(waypoint).autoware_from_world_coords() ) - self.route_node.request_route(goal_pose, waypoints) + self.route_node.request_route( + goal_pose.autoware_from_world_coords(), waypoints + ) # check if the current route is set if self.autoware_state.route_ready() and not self.autoware_state.sent_engage: diff --git a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py index 835fc3d..cd80555 100644 --- a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py +++ b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py @@ -6,6 +6,8 @@ from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion +import math + class Waypoint(object): def __init__(self, x, y, z, node=None): @@ -14,7 +16,7 @@ def __init__(self, x, y, z, node=None): self.z = z self.node = node - self.marker_pub = node.marker_pub + self.marker_pub = node.marker_publisher self.client = CarlaDataProvider.get_client() @@ -42,7 +44,9 @@ def autoware_from_world_coords(self) -> Pose: ) qx, qy, qz, qw = quaternion_from_euler( - orientation.roll, orientation.pitch, round(orientation.yaw, 1) + math.radians(orientation.roll), + -math.radians(orientation.pitch), + -math.radians(round(orientation.yaw, 1)), ) pose.position = ros_point @@ -55,10 +59,6 @@ def autoware_from_world_coords(self) -> Pose: self.pose = pose - if self.marker_pub is not None: - print(self) - self.marker_pub.publish(self._publish_marker(pose)) - return pose def _publish_marker(self, pose): From b59268bf7456810a6fcfb3e3fe2a4bc9b52b083b Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Tue, 5 Aug 2025 13:26:28 +0100 Subject: [PATCH 3/3] bug fixes --- srunner/autoagents/autoware_agent.py | 10 ++++------ .../autoware_nodes/autoware_types/waypoint.py | 14 ++++++++++---- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index e2383dd..8dd9e5a 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -127,17 +127,15 @@ def run_step(self) -> None: if self.autoware_state.is_ready_publish_route() and self.agent_set_route: waypoints = [] - goal_pose = self._convert_to_waypoint(self.goal_pose_world) - - goal_pose._publish_marker(goal_pose.autoware_from_world_coords()) + goal_pose = self._convert_to_waypoint( + self.goal_pose_world + ).autoware_from_world_coords() for waypoint in self.waypoints_world: waypoints.append( self._convert_to_waypoint(waypoint).autoware_from_world_coords() ) - self.route_node.request_route( - goal_pose.autoware_from_world_coords(), waypoints - ) + self.route_node.request_route(goal_pose, waypoints) # check if the current route is set if self.autoware_state.route_ready() and not self.autoware_state.sent_engage: diff --git a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py index cd80555..1cf7e47 100644 --- a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py +++ b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py @@ -7,6 +7,7 @@ from geometry_msgs.msg import Quaternion import math +import random class Waypoint(object): @@ -14,6 +15,7 @@ def __init__(self, x, y, z, node=None): self.x = x self.y = y self.z = z + self.id = random.randint(0, 10000) self.node = node self.marker_pub = node.marker_publisher @@ -59,20 +61,24 @@ def autoware_from_world_coords(self) -> Pose: self.pose = pose + if self.marker_pub is not None: + self.marker_pub.publish(self._publish_marker(pose)) + return pose def _publish_marker(self, pose): marker = Marker() - marker.header.frame_id = "/map" + marker.header.frame_id = "map" marker.header.stamp = self.node.get_clock().now().to_msg() - marker.type = marker.ARROW + marker.type = marker.SPHERE marker.action = marker.ADD + marker.id = self.id marker.pose = pose - marker.scale.x = 0.5 - marker.scale.y = 0.05 + marker.scale.x = 1.0 + marker.scale.y = 0.4 marker.scale.z = 1.0 marker.color.r = 1.0