feat(planning_debug_tools): add recorded ego pose publisher to percep…

…tion replayer (autowarefoundation#4906)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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 PoseWithCovarianceStamped
 from perception_replayer_common import PerceptionReplayerCommon
 import rclpy
 from time_manager_widget import TimeManagerWidget
@@ -43,6 +44,7 @@ def __init__(self, args):
         self.widget.button.clicked.connect(self.onPushed)
         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)
 
         # start timer callback
         self.delta_time = 0.1
@@ -118,6 +120,57 @@ def onPushed(self, event):
     def onSetRate(self, button):
         self.rate = float(button.text())
 
+    def publish_recorded_ego_pose(self):
+        ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp)
+        if not ego_odom:
+            return
+
+        recorded_ego_pose = PoseWithCovarianceStamped()
+        recorded_ego_pose.header.stamp = self.get_clock().now().to_msg()
+        recorded_ego_pose.header.frame_id = "map"
+        recorded_ego_pose.pose.pose = ego_odom.pose.pose
+        recorded_ego_pose.pose.covariance = [
+            0.25,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.25,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.06853892326654787,
+        ]
+
+        self.recorded_ego_pub.publish(recorded_ego_pose)
+        print("Published recorded ego pose as /initialpose")
+
 
 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 PoseWithCovarianceStamped
 from nav_msgs.msg import Odometry
 import psutil
 from rclpy.node import Node
@@ -66,6 +67,7 @@ def __init__(self, args, name):
         self.pointcloud_pub = self.create_publisher(
             PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1
         )
+        self.recorded_ego_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1)
 
         # load rosbag
         print("Stared loading rosbag")

planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py

@@ -89,13 +89,15 @@ def setupUI(self):
         self.button.setCheckable(True)
         self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
         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)
 
         # 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, 2, 0, 1, -1)
+        self.grid_layout.addWidget(self.slider, 3, 0, 1, -1)
 
         self.setCentralWidget(self.central_widget)