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 498f7e45..5f9e9575 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 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() 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 71f15375..a30ac9b2 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 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): 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 a1d87a8b..c9ec36ca 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 @@ -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)