From c3602bf3621a8b819b048079df54071c5d8b1125 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 15 Mar 2024 16:20:09 +0900 Subject: [PATCH] feat(perception_replayer): add a button to publish the goal pose Signed-off-by: Maxime CLEMENT --- .../perception_replayer/perception_replayer.py | 12 +++++++++++- .../perception_replayer_common.py | 6 +++++- .../perception_replayer/time_manager_widget.py | 4 +++- 3 files changed, 19 insertions(+), 3 deletions(-) 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..3977c2cd 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -20,7 +20,7 @@ import sys from PyQt5.QtWidgets import QApplication -from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped from perception_replayer_common import PerceptionReplayerCommon import rclpy from time_manager_widget import TimeManagerWidget @@ -46,6 +46,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 +173,15 @@ 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..73bc3a4e 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,7 +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 geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped from nav_msgs.msg import Odometry import psutil from rclpy.node import Node @@ -76,6 +76,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)