Skip to content

Commit b2836df

Browse files
satoshi-otasatoshi-ota
committed
EXPERIMENT: ADD LATEST DEBUG TOOL
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent d4d86de commit b2836df

File tree

6 files changed

+818
-1
lines changed

6 files changed

+818
-1
lines changed

‎planning/planning_debug_tools/CMakeLists.txt

+2-1
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ ament_auto_package(
5050
install(PROGRAMS
5151
scripts/trajectory_visualizer.py
5252
scripts/closest_velocity_checker.py
53-
scripts/perception_reproducer.py
53+
scripts/perception_replayer/perception_reproducer.py
54+
scripts/perception_replayer/perception_replayer.py
5455
DESTINATION lib/${PROJECT_NAME}
5556
)

‎planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py

+203
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,203 @@
1+
#!/usr/bin/env python3
2+
3+
# Copyright 2023 TIER IV, Inc.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
17+
import argparse
18+
import copy
19+
import functools
20+
import sys
21+
22+
from PyQt5.QtWidgets import QApplication
23+
from geometry_msgs.msg import PoseWithCovarianceStamped
24+
from perception_replayer_common import PerceptionReplayerCommon
25+
import rclpy
26+
from time_manager_widget import TimeManagerWidget
27+
from utils import create_empty_pointcloud
28+
from utils import translate_objects_coordinate
29+
30+
31+
class PerceptionReplayer(PerceptionReplayerCommon):
32+
def __init__(self, args):
33+
super().__init__(args, "perception_replayer")
34+
35+
self.bag_timestamp = self.rosbag_objects_data[0][0]
36+
self.is_pause = False
37+
self.rate = 1.0
38+
self.prev_traffic_signals_msg = None
39+
40+
# initialize widget
41+
self.widget = TimeManagerWidget(
42+
self.rosbag_objects_data[0][0], self.rosbag_objects_data[-1][0]
43+
)
44+
self.widget.show()
45+
self.widget.button.clicked.connect(self.onPushed)
46+
for button in self.widget.rate_button:
47+
button.clicked.connect(functools.partial(self.onSetRate, button))
48+
self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose)
49+
50+
# start timer callback
51+
self.delta_time = 0.1
52+
self.timer = self.create_timer(self.delta_time, self.on_timer)
53+
print("Start timer callback")
54+
55+
def on_timer(self):
56+
timestamp = self.get_clock().now().to_msg()
57+
58+
self.kill_online_perception_node()
59+
60+
if self.args.detected_object:
61+
pointcloud_msg = create_empty_pointcloud(timestamp)
62+
self.pointcloud_pub.publish(pointcloud_msg)
63+
64+
# step timestamp
65+
# get timestamp from slider
66+
self.bag_timestamp = self.rosbag_objects_data[0][
67+
0
68+
] + self.widget.slider.value() / 1000000 * (
69+
self.rosbag_objects_data[-1][0] - self.rosbag_objects_data[0][0]
70+
)
71+
if not self.is_pause:
72+
self.bag_timestamp += self.rate * self.delta_time * 1e9 # seconds to timestamp
73+
# update slider value from the updated timestamp
74+
self.widget.slider.setValue(self.widget.timestamp_to_value(self.bag_timestamp))
75+
76+
# extract message by the timestamp
77+
msgs = copy.deepcopy(self.find_topics_by_timestamp(self.bag_timestamp))
78+
objects_msg = msgs[0]
79+
traffic_signals_msg = msgs[1]
80+
81+
# objects
82+
if objects_msg:
83+
objects_msg.header.stamp = timestamp
84+
if self.args.detected_object:
85+
if not self.ego_pose:
86+
print("No ego pose found.")
87+
return
88+
89+
ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp)
90+
if not ego_odom:
91+
return
92+
log_ego_pose = ego_odom.pose.pose
93+
94+
translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg)
95+
self.objects_pub.publish(objects_msg)
96+
97+
# traffic signals
98+
# temporary support old auto msgs
99+
if traffic_signals_msg:
100+
if self.is_auto_traffic_signals:
101+
traffic_signals_msg.header.stamp = timestamp
102+
self.auto_traffic_signals_pub.publish(traffic_signals_msg)
103+
else:
104+
traffic_signals_msg.stamp = timestamp
105+
self.traffic_signals_pub.publish(traffic_signals_msg)
106+
self.prev_traffic_signals_msg = traffic_signals_msg
107+
elif self.prev_traffic_signals_msg:
108+
if self.is_auto_traffic_signals:
109+
self.prev_traffic_signals_msg.header.stamp = timestamp
110+
self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg)
111+
else:
112+
self.prev_traffic_signals_msg.stamp = timestamp
113+
self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
114+
115+
def onPushed(self, event):
116+
if self.widget.button.isChecked():
117+
self.is_pause = True
118+
else:
119+
self.is_pause = False
120+
121+
def onSetRate(self, button):
122+
self.rate = float(button.text())
123+
124+
def publish_recorded_ego_pose(self):
125+
ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp)
126+
if not ego_odom:
127+
return
128+
129+
recorded_ego_pose = PoseWithCovarianceStamped()
130+
recorded_ego_pose.header.stamp = self.get_clock().now().to_msg()
131+
recorded_ego_pose.header.frame_id = "map"
132+
recorded_ego_pose.pose.pose = ego_odom.pose.pose
133+
recorded_ego_pose.pose.covariance = [
134+
0.25,
135+
0.0,
136+
0.0,
137+
0.0,
138+
0.0,
139+
0.0,
140+
0.0,
141+
0.25,
142+
0.0,
143+
0.0,
144+
0.0,
145+
0.0,
146+
0.0,
147+
0.0,
148+
0.0,
149+
0.0,
150+
0.0,
151+
0.0,
152+
0.0,
153+
0.0,
154+
0.0,
155+
0.0,
156+
0.0,
157+
0.0,
158+
0.0,
159+
0.0,
160+
0.0,
161+
0.0,
162+
0.0,
163+
0.0,
164+
0.0,
165+
0.0,
166+
0.0,
167+
0.0,
168+
0.0,
169+
0.06853892326654787,
170+
]
171+
172+
self.recorded_ego_pub_as_initialpose.publish(recorded_ego_pose)
173+
print("Published recorded ego pose as /initialpose")
174+
175+
176+
if __name__ == "__main__":
177+
parser = argparse.ArgumentParser()
178+
parser.add_argument("-b", "--bag", help="rosbag", default=None)
179+
parser.add_argument(
180+
"-d", "--detected-object", help="publish detected object", action="store_true"
181+
)
182+
parser.add_argument(
183+
"-t", "--tracked-object", help="publish tracked object", action="store_true"
184+
)
185+
parser.add_argument(
186+
"-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3"
187+
)
188+
args = parser.parse_args()
189+
190+
app = QApplication(sys.argv)
191+
192+
rclpy.init()
193+
node = PerceptionReplayer(args)
194+
195+
try:
196+
while True:
197+
app.processEvents()
198+
rclpy.spin_once(node, timeout_sec=0.01)
199+
except KeyboardInterrupt:
200+
pass
201+
finally:
202+
node.destroy_node()
203+
rclpy.shutdown()

‎planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py

+183
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,183 @@
1+
#!/usr/bin/env python3
2+
3+
# Copyright 2023 TIER IV, Inc.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
17+
import os
18+
from subprocess import CalledProcessError
19+
from subprocess import check_output
20+
import time
21+
22+
from autoware_auto_perception_msgs.msg import DetectedObjects
23+
from autoware_auto_perception_msgs.msg import PredictedObjects
24+
from autoware_auto_perception_msgs.msg import TrackedObjects
25+
from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray
26+
from autoware_perception_msgs.msg import TrafficSignalArray
27+
from geometry_msgs.msg import PoseWithCovarianceStamped
28+
from nav_msgs.msg import Odometry
29+
import psutil
30+
from rclpy.node import Node
31+
from rclpy.serialization import deserialize_message
32+
from rosbag2_py import StorageFilter
33+
from rosidl_runtime_py.utilities import get_message
34+
from sensor_msgs.msg import PointCloud2
35+
from utils import open_reader
36+
37+
38+
class PerceptionReplayerCommon(Node):
39+
def __init__(self, args, name):
40+
super().__init__(name)
41+
self.args = args
42+
43+
self.ego_pose = None
44+
self.rosbag_objects_data = []
45+
self.rosbag_ego_odom_data = []
46+
self.rosbag_traffic_signals_data = []
47+
self.is_auto_traffic_signals = False
48+
49+
# subscriber
50+
self.sub_odom = self.create_subscription(
51+
Odometry, "/localization/kinematic_state", self.on_odom, 1
52+
)
53+
54+
# publisher
55+
if self.args.detected_object:
56+
self.objects_pub = self.create_publisher(
57+
DetectedObjects, "/perception/object_recognition/detection/objects", 1
58+
)
59+
elif self.args.tracked_object:
60+
self.objects_pub = self.create_publisher(
61+
TrackedObjects, "/perception/object_recognition/tracking/objects", 1
62+
)
63+
else:
64+
self.objects_pub = self.create_publisher(
65+
PredictedObjects, "/perception/object_recognition/objects", 1
66+
)
67+
68+
self.pointcloud_pub = self.create_publisher(
69+
PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1
70+
)
71+
self.recorded_ego_pub_as_initialpose = self.create_publisher(
72+
PoseWithCovarianceStamped, "/initialpose", 1
73+
)
74+
75+
self.recorded_ego_pub = self.create_publisher(
76+
Odometry, "/perception_reproducer/rosbag_ego_odom", 1
77+
)
78+
79+
# load rosbag
80+
print("Stared loading rosbag")
81+
if os.path.isdir(args.bag):
82+
for bag_file in sorted(os.listdir(args.bag)):
83+
if bag_file.endswith(self.args.rosbag_format):
84+
self.load_rosbag(args.bag + "/" + bag_file)
85+
else:
86+
self.load_rosbag(args.bag)
87+
print("Ended loading rosbag")
88+
89+
# temporary support old auto msgs
90+
if self.is_auto_traffic_signals:
91+
self.auto_traffic_signals_pub = self.create_publisher(
92+
AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1
93+
)
94+
else:
95+
self.traffic_signals_pub = self.create_publisher(
96+
TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1
97+
)
98+
99+
# wait for ready to publish/subscribe
100+
time.sleep(1.0)
101+
102+
def on_odom(self, odom):
103+
self.ego_pose = odom.pose.pose
104+
105+
def load_rosbag(self, rosbag2_path: str):
106+
reader = open_reader(str(rosbag2_path))
107+
108+
topic_types = reader.get_all_topics_and_types()
109+
# Create a map for quicker lookup
110+
type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))}
111+
112+
objects_topic = (
113+
"/perception/object_recognition/detection/objects"
114+
if self.args.detected_object
115+
else "/perception/object_recognition/tracking/objects"
116+
if self.args.tracked_object
117+
else "/perception/object_recognition/objects"
118+
)
119+
ego_odom_topic = "/localization/kinematic_state"
120+
traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals"
121+
topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic])
122+
reader.set_filter(topic_filter)
123+
124+
while reader.has_next():
125+
(topic, data, stamp) = reader.read_next()
126+
msg_type = get_message(type_map[topic])
127+
msg = deserialize_message(data, msg_type)
128+
if topic == objects_topic:
129+
self.rosbag_objects_data.append((stamp, msg))
130+
if topic == ego_odom_topic:
131+
self.rosbag_ego_odom_data.append((stamp, msg))
132+
if topic == traffic_signals_topic:
133+
self.rosbag_traffic_signals_data.append((stamp, msg))
134+
self.is_auto_traffic_signals = (
135+
"autoware_auto_perception_msgs" in type(msg).__module__
136+
)
137+
138+
def kill_online_perception_node(self):
139+
# kill node if required
140+
kill_process_name = None
141+
if self.args.detected_object:
142+
kill_process_name = "dummy_perception_publisher_node"
143+
elif self.args.tracked_object:
144+
kill_process_name = "multi_object_tracker"
145+
else:
146+
kill_process_name = "map_based_prediction"
147+
if kill_process_name:
148+
try:
149+
pid = check_output(["pidof", kill_process_name])
150+
process = psutil.Process(int(pid[:-1]))
151+
process.terminate()
152+
except CalledProcessError:
153+
pass
154+
155+
def binary_search(self, data, timestamp):
156+
if data[-1][0] < timestamp:
157+
return data[-1][1]
158+
elif data[0][0] > timestamp:
159+
return data[0][1]
160+
161+
low, high = 0, len(data) - 1
162+
163+
while low <= high:
164+
mid = (low + high) // 2
165+
if data[mid][0] < timestamp:
166+
low = mid + 1
167+
elif data[mid][0] > timestamp:
168+
high = mid - 1
169+
else:
170+
return data[mid][1]
171+
172+
# Return the next timestamp's data if available
173+
if low < len(data):
174+
return data[low][1]
175+
return None
176+
177+
def find_topics_by_timestamp(self, timestamp):
178+
objects_data = self.binary_search(self.rosbag_objects_data, timestamp)
179+
traffic_signals_data = self.binary_search(self.rosbag_traffic_signals_data, timestamp)
180+
return objects_data, traffic_signals_data
181+
182+
def find_ego_odom_by_timestamp(self, timestamp):
183+
return self.binary_search(self.rosbag_ego_odom_data, timestamp)

0 commit comments

Comments
 (0)