Merge pull request #55 from Intelligent-Testing-Lab/49-lidar-pointcloud-is-0

49 lidar pointcloud is 0

Author: david-gasinski

Dockerfile

@@ -36,7 +36,7 @@ RUN mkdir /cyclonedds && \
     mv /autoware_scenario_runner/docker/cyclonedds.xml /cyclonedds/ && \
     echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> ~/.bashrc && \
     echo "export CYCLONEDDS_URI=file:///cyclonedds/cyclonedds.xml" >> ~/.bashrc && \
-    echo "alias rossrc='source ${AUTOWARE_MSG_PKG}} && source ${ROS_PKG}} && echo Sourced'" >> ~/.bashrc && \
+    echo "alias rossrc='source ${AUTOWARE_MSG_PKG} && source ${ROS_PKG} && echo Sourced'" >> ~/.bashrc && \
     source ~/.bashrc
 
 ENV CARLA_API_ROOT="/autoware_scenario_runner/PythonAPI"

aw_scenario_runner.py

@@ -146,7 +146,7 @@ def run_scenario(
         self.ego_vehicles.append(ego.spawn())
         logger.info("Spawned ego...")
 
-        self.carla_world.tick()
+        self.carla_world.tick()  # client must tick to spawn actors
 
         logger.info("Setting up sensor configuration...")
         ego.setup_sensors()

example_scenario.json

@@ -34,23 +34,30 @@
                 "waypoints": [
                     {
                         "position": {
-                            "x": 983.5,
-                            "y": 5382.2,
-                            "z": 371
+                            "x": 88.4,
+                            "y": 82.2,
+                            "z": 0.0
                         }
                     },
                     {
                         "position": {
-                            "x": 1234,
-                            "y": 5.2,
-                            "z": 3123471
+                            "x": 230.0,
+                            "y": 133.5,
+                            "z": 0.0
                         }
                     },
                     {
                         "position": {
-                            "x": 123412343.5,
-                            "y": 512341234.2,
-                            "z": 3712341234
+                            "x": 209.5,
+                            "y": 195.1,
+                            "z": 0.0
+                        }
+                    },
+                    {
+                        "position": {
+                            "x": 88.4,
+                            "y": 260.7,
+                            "z": 0.0
                         }
                     }
                 ],

srunner/autoagents/agent_state/autoware_state.py

@@ -31,6 +31,9 @@ def is_ready_publish_route(self) -> bool:
     def is_planning(self) -> bool:
         return self.route_state == 3
 
+    def route_ready(self) -> bool:
+        return self.sent_route and self.route_set()
+
     def reset_state(self) -> None:
         # internal message states
         self.sent_route: bool = False

srunner/autoagents/agent_state/state.py

@@ -23,6 +23,3 @@ def __repr__(self):
     def __str__(self):
         """Return name of state and list of attributes"""
         return f"{self.__class__.__name__} state: {self.__dict__}"
-
-    def route_ready(self) -> None:
-        pass

srunner/autoagents/autoware_agent.py

@@ -38,7 +38,7 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
 
         self.autoware_state = autoware_state.AutowareState("ego_vehicle", None)
 
-        self.route_node = route_node.RouteNode()
+        self.route_node = route_node.RouteNode(self.autoware_state)
         self.state_node = state_node.StateNode(self.autoware_state)
         self.autoware_node = autoware_node.AutowareNode(self.autoware_state)
 
@@ -87,7 +87,7 @@ def set_route(self) -> None:
 
         # reinitialise localization
         logger.info("Localising Autoware agent...")
-        self.autoware_node.request_localize()  # None uses GNSS
+        # self.autoware_node.request_localize()  # None uses GNSS
 
         logger.info("Clearing route...")
         self.route_node.request_clear_route()
@@ -135,7 +135,6 @@ def run_step(self) -> None:
                     self._convert_to_waypoint(waypoint).autoware_from_world_coords()
                 )
             self.route_node.request_route(goal_pose, waypoints)
-            self.autoware_state.sent_route = True
 
         # check if the current route is set
         if self.autoware_state.route_ready() and not self.autoware_state.sent_engage:

srunner/autoagents/autoware_nodes/autoware_node.py

@@ -90,7 +90,7 @@ def request_localize(
             # Example: request.pose = PoseWithCovarianceStamped() # Creates an empty message
 
         # Call the service asynchronously
-        future = self.localize_client.call(request)
+        future = self.localize_client.call_async(request)
 
         # Add a callback to process the response when it arrives
         future.add_done_callback(self.localize_response_callback)

srunner/autoagents/autoware_nodes/autoware_types/waypoint.py

@@ -1,5 +1,7 @@
 from tf_transformations import quaternion_from_euler
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Quaternion
 
 
 class Waypoint(object):
@@ -12,13 +14,25 @@ def __init__(self, x, y, z, yaw):
         # rounding is need by autoware to function properly
         self.quaternion = quaternion_from_euler(0, 0, round(self.yaw, 1))
 
-    def autoware_from_world_coords(self) -> PoseStamped:
+    def autoware_from_world_coords(self) -> Pose:
         """convert from carla world coordinates to autoware waypoints
 
         Returns:
             PoseStamped: position and time stamp
         """
-        pose = PoseStamped()
+        pose = Pose()
 
-        pose.header.frame_id = "map"
-        return
+        ros_point = Point()
+        ros_point.x = self.x
+        ros_point.y = -self.y
+        ros_point.z = self.z
+
+        pose.position = ros_point
+        pose.orientation = Quaternion(
+            w=self.quaternion[3],
+            x=self.quaternion[0],
+            y=self.quaternion[1],
+            z=self.quaternion[2],
+        )
+
+        return pose

srunner/autoagents/autoware_nodes/route_node.py

@@ -18,10 +18,12 @@ class RouteNode(Node):
     set_route_points_service_name = "/api/routing/set_route_points"
     clear_route_service_name = "/api/routing/clear_route"
 
-    def __init__(self) -> None:
+    def __init__(self, autoware_state) -> None:
         # Initialize the Node base class with a unique name
         super().__init__("route_client_node")
 
+        self.autoware_state = autoware_state
+
         # Create service clients, not publishers
         self.set_route_client = self.create_client(
             SetRoutePoints, self.set_route_points_service_name
@@ -81,7 +83,7 @@ def request_route(self, goal: Pose, waypoints: list[Pose]) -> None:
         request.waypoints = waypoints
 
         # Call the service asynchronously. This returns a Future object.
-        future = self.set_route_client.call(request)
+        future = self.set_route_client.call_async(request)
 
         # Add a callback to process the response when it arrives.
         future.add_done_callback(self.set_route_response_callback)
@@ -93,6 +95,7 @@ def set_route_response_callback(self, future):
             response = future.result()
             # Assuming Autoware services return a status field with success/message
             if response.status.success:
+                self.autoware_state.sent_route = True
                 self.get_logger().info("Route set successfully!")
             else:
                 self.get_logger().warn(
@@ -109,7 +112,7 @@ def request_clear_route(self):
         request = ClearRoute_Request()  # Or ClearRoute()
 
         # Call the service asynchronously
-        future = self.clear_route_client.call(request)
+        future = self.clear_route_client.call_async(request)
         future.add_done_callback(self.clear_route_response_callback)
 
     def clear_route_response_callback(self, future):

srunner/autoagents/autoware_nodes/state_node.py

@@ -5,6 +5,10 @@
 from srunner.autoagents.agent_state import autoware_state
 from autoware_carla_interface_msgs.msg import EgoConfig, BridgeState
 
+import logging
+
+logger = logging.getLogger("scenario-runner")
+
 
 class StateNode(Node):
     route_state = "/planning/mission_planning/state"
@@ -46,6 +50,7 @@ def bridge_state_cb(self, bridge_state_msg: BridgeState):
             bridge_state_msg (BridgeState): bridge state boolean value
         """
         self.autoware_state.bridge_ready = bridge_state_msg.bridge_ready
+        logger.info(f"Bridge ready state: {self.autoware_state.bridge_ready}")
 
     def route_state_cb(self, route_state_msg: RouteState) -> None:
         """Set the AutowareState attribute route_state
@@ -54,14 +59,17 @@ def route_state_cb(self, route_state_msg: RouteState) -> None:
             route_state_msg (RouteState): route state message received
         """
         self.autoware_state.route_state = route_state_msg.state
+        logger.info(f"Route state: {self.autoware_state.route_state}")
 
     def motion_state_cb(self, motion_state_msg: MotionState) -> None:
         """set the AutowareState attribute motion_state
 
         Args:
             motion_state_msg (MotionState): motion state message received
         """
+
         self.autoware_state.motion_state = motion_state_msg.state
+        logger.info(f"Motion state: {self.autoware_state.motion_state}")
 
     def localize_state_cb(
         self, localize_state_msg: LocalizationInitializationState
@@ -72,3 +80,4 @@ def localize_state_cb(
             localize_state_msg (LocalizationInitializationState): localization message received
         """
         self.autoware_state.localize_state = localize_state_msg.state
+        logger.info(f"Localization state: {self.autoware_state.localize_state}")