feat(perception_replayer): add a button to publish the goal pose (#15)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py

@@ -20,6 +20,7 @@
 import sys
 
 from PyQt5.QtWidgets import QApplication
+from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from perception_replayer_common import PerceptionReplayerCommon
 import rclpy
@@ -46,6 +47,7 @@ def __init__(self, args):
         for button in self.widget.rate_button:
             button.clicked.connect(functools.partial(self.onSetRate, button))
         self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose)
+        self.widget.pub_goal_pose_button.clicked.connect(self.publish_goal)
 
         # start timer callback
         self.delta_time = 0.1
@@ -172,6 +174,17 @@ def publish_recorded_ego_pose(self):
         self.recorded_ego_pub_as_initialpose.publish(recorded_ego_pose)
         print("Published recorded ego pose as /initialpose")
 
+    def publish_goal(self):
+        if not self.rosbag_ego_odom_data:
+            return
+
+        goal_pose = PoseStamped()
+        goal_pose.header.stamp = self.get_clock().now().to_msg()
+        goal_pose.header.frame_id = "map"
+        goal_pose.pose = self.rosbag_ego_odom_data[-1][1].pose.pose
+        self.goal_pose_publisher.publish(goal_pose)
+        print("Published last recorded ego pose as /planning/mission_planning/goal")
+
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py

@@ -24,6 +24,7 @@
 from autoware_auto_perception_msgs.msg import TrackedObjects
 from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray
 from autoware_perception_msgs.msg import TrafficSignalArray
+from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from nav_msgs.msg import Odometry
 import psutil
@@ -76,6 +77,10 @@ def __init__(self, args, name):
             Odometry, "/perception_reproducer/rosbag_ego_odom", 1
         )
 
+        self.goal_pose_publisher = self.create_publisher(
+            PoseStamped, "/planning/mission_planning/goal", 1
+        )
+
         # load rosbag
         print("Stared loading rosbag")
         if os.path.isdir(args.bag):

planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py

@@ -91,13 +91,15 @@ def setupUI(self):
         self.grid_layout.addWidget(self.button, 1, 0, 1, -1)
         self.pub_recorded_ego_pose_button = QPushButton("publish recorded ego pose")
         self.grid_layout.addWidget(self.pub_recorded_ego_pose_button, 2, 0, 1, -1)
+        self.pub_goal_pose_button = QPushButton("publish last recorded ego pose as goal pose")
+        self.grid_layout.addWidget(self.pub_goal_pose_button, 3, 0, 1, -1)
 
         # slider
         self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value)
         self.slider.setMinimum(0)
         self.slider.setMaximum(self.max_value)
         self.slider.setValue(0)
-        self.grid_layout.addWidget(self.slider, 3, 0, 1, -1)
+        self.grid_layout.addWidget(self.slider, 4, 0, 1, -1)
 
         self.setCentralWidget(self.central_widget)