diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index a9abc5be60761..f1015be07fd7c 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/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() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 33cf8ca1f16ee..f59270a9740af 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/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") diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py index 382e725b114cd..a1d87a8b06266 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py +++ b/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)