From c3602bf3621a8b819b048079df54071c5d8b1125 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 15 Mar 2024 16:20:09 +0900 Subject: [PATCH 1/2] 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) From 4770843d3e73212bbde1039a5b3f7cb66361c4dd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 15 Mar 2024 07:24:09 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../scripts/perception_replayer/perception_replayer.py | 7 +++++-- .../perception_replayer/perception_replayer_common.py | 3 ++- 2 files changed, 7 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 3977c2cd..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,7 +20,8 @@ import sys from PyQt5.QtWidgets import QApplication -from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped from perception_replayer_common import PerceptionReplayerCommon import rclpy from time_manager_widget import TimeManagerWidget @@ -174,7 +175,8 @@ def publish_recorded_ego_pose(self): print("Published recorded ego pose as /initialpose") def publish_goal(self): - if (not self.rosbag_ego_odom_data): return + if not self.rosbag_ego_odom_data: + return goal_pose = PoseStamped() goal_pose.header.stamp = self.get_clock().now().to_msg() @@ -183,6 +185,7 @@ def publish_goal(self): self.goal_pose_publisher.publish(goal_pose) print("Published last recorded ego pose as /planning/mission_planning/goal") + if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("-b", "--bag", help="rosbag", default=None) 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 73bc3a4e..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,7 +24,8 @@ 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, PoseStamped +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import psutil from rclpy.node import Node