Merge pull request #66 from Intelligent-Testing-Lab/64-explore-alternative-routing-technique

Call publish_route() only once

Author: Gwilym-Rutherford

srunner/autoagents/autoware_agent.py

@@ -37,7 +37,6 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
 
         rclpy.init(args=None)
 
-
         self.autoware_state = autoware_state.AutowareState("ego_vehicle", None)
 
         self.route_node = route_node.RouteNode(self.autoware_state)
@@ -55,6 +54,8 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
         )
         self._executor_thread.start()
 
+        self.sent_route = False
+
         # check the bridge is ready
         # publish sensor information to the bridge
         # wait for it to return the correct message
@@ -93,8 +94,6 @@ def set_route(self) -> None:
 
         logger.info("Clearing route...")
         self.route_node.request_clear_route()
-        
-        
 
     def _convert_to_waypoint(self, point):
         """Returns a waypoint
@@ -129,7 +128,11 @@ def run_step(self) -> None:
         if not self.agent_set_route:
             self.set_route()
 
-        if self.autoware_state.is_ready_publish_route() and self.agent_set_route:
+        if (
+            self.autoware_state.is_ready_publish_route()
+            and self.agent_set_route
+            and not self.sent_route
+        ):
             waypoints = []
             goal_pose = self._convert_to_waypoint(
                 self.goal_pose_world
@@ -139,8 +142,9 @@ def run_step(self) -> None:
                 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, waypoints)
             self.route_node.publish_route(goal_pose, waypoints)
+            self.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/route_node.py

@@ -101,22 +101,26 @@ def request_route(self, goal: Pose, waypoints: list[Pose]) -> None:
 
     def publish_route(self, goal: Pose, checkpoints: list[Pose]) -> None:
         self._publish_goal(goal)
-        
+        self.node.create_rate(1.0).sleep()
         for checkpoint in checkpoints:
             self._publish_checkpoint(checkpoint)
 
-    def _publish_goal(self, goal) -> None:
+    def _publish_goal(self, goal_point) -> None:
         goal = PoseStamped()
         goal.header.stamp = self.node.get_clock().now().to_msg()
-        goal.pose = goal
+        goal.header.frame_id = "map"
+        
+        goal.pose = goal_point
         self.goal_publisher.publish(goal)
 
 
     def _publish_checkpoint(self, checkpoint) -> None:
-        goal = PoseStamped()
-        goal.header.stamp = self.node.get_clock().now().to_msg()
-        goal.pose = checkpoint
-        self.checkpoint_publisher.publish(goal)
+        checkpoint_msg = PoseStamped()
+        checkpoint_msg.header.stamp = self.node.get_clock().now().to_msg()
+        checkpoint_msg.header.frame_id = "map"
+        
+        checkpoint_msg.pose = checkpoint
+        self.checkpoint_publisher.publish(checkpoint_msg)
 
     def set_route_response_callback(self, future):
         """Callback to handle the response from the SetRoutePoints service."""