style(pre-commit): autofix

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py

@@ -105,7 +105,6 @@ def on_timer(self):
             self.prev_traffic_signals_msg.stamp = timestamp
             self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
 
-
     def onPushed(self, event):
         if self.widget.button.isChecked():
             self.is_pause = True

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py

@@ -19,9 +19,12 @@
 from subprocess import check_output
 import time
 
-from autoware_perception_msgs.msg import DetectedObjects, PredictedObjects, TrackedObjects
-from autoware_perception_msgs.msg import TrafficLightGroupArray, TrafficLightGroup, TrafficLightElement
-
+from autoware_perception_msgs.msg import DetectedObjects
+from autoware_perception_msgs.msg import PredictedObjects
+from autoware_perception_msgs.msg import TrackedObjects
+from autoware_perception_msgs.msg import TrafficLightElement
+from autoware_perception_msgs.msg import TrafficLightGroup
+from autoware_perception_msgs.msg import TrafficLightGroupArray
 from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from nav_msgs.msg import Odometry
@@ -31,7 +34,8 @@
 from rosbag2_py import StorageFilter
 from rosidl_runtime_py.utilities import get_message
 from sensor_msgs.msg import PointCloud2
-from utils import open_reader,get_starting_time
+from utils import get_starting_time
+from utils import open_reader
 
 
 class PerceptionReplayerCommon(Node):
@@ -85,9 +89,13 @@ def __init__(self, args, name):
         # load rosbag
         print("Stared loading rosbag")
         if os.path.isdir(args.bag):
-            bags = [ os.path.join(args.bag, base_name) for base_name in os.listdir(args.bag) if base_name.endswith(args.rosbag_format) ]
+            bags = [
+                os.path.join(args.bag, base_name)
+                for base_name in os.listdir(args.bag)
+                if base_name.endswith(args.rosbag_format)
+            ]
             for bag_file in sorted(bags, key=get_starting_time):
-                    self.load_rosbag(bag_file)
+                self.load_rosbag(bag_file)
         else:
             self.load_rosbag(args.bag)
         print("Ended loading rosbag")
@@ -126,7 +134,9 @@ def load_rosbag(self, rosbag2_path: str):
                     # convert old autoware_auto_perception_msgs to new autoware_perception_msgs
                     new_msg = self.objects_pub.msg_type()
                     for field in msg.__slots__:
-                        setattr(new_msg, field, getattr(msg, field))# it's unsafe because the elements inside the message are still the old type, but it works for now on.
+                        setattr(
+                            new_msg, field, getattr(msg, field)
+                        )  # it's unsafe because the elements inside the message are still the old type, but it works for now on.
                     msg = new_msg
                 self.rosbag_objects_data.append((stamp, msg))
             if topic == ego_odom_topic:

planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py

@@ -115,7 +115,6 @@ def on_timer(self):
             self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
         self.stopwatch.toc("transform and publish")
 
-
         self.stopwatch.toc("total on_timer")
 
     def find_nearest_ego_odom_by_observation(self, ego_pose):

planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer_v2.py

@@ -15,8 +15,8 @@
 # limitations under the License.
 
 import argparse
-import pickle
 from collections import deque
+import pickle
 
 import numpy as np
 from perception_replayer_common import PerceptionReplayerCommon
@@ -25,28 +25,33 @@
 from utils import create_empty_pointcloud
 from utils import translate_objects_coordinate
 
-dist_eps = 1e-2 # (meters)
+dist_eps = 1e-2  # (meters)
+
 
 class PerceptionReproducerV2(PerceptionReplayerCommon):
-    '''
+    """
     PerceptionReproducerV2 is used reproduces perception topic data from rosbag, specifically optimized for use with simulators.
-    '''
+    """
+
     def __init__(self, args):
-        self.rosbag_ego_odom_seach_radius = args.seach_radius # (m) the range of the ego odom to search, 
-        self.reproduce_cooldown = args.reproduce_cooldown # (sec) the cooldown time for republishing published data, please make sure that it's greater than the ego's stopping time. 
-
+        self.rosbag_ego_odom_seach_radius = (
+            args.seach_radius
+        )  # (m) the range of the ego odom to search,
+        self.reproduce_cooldown = (
+            args.reproduce_cooldown
+        )  # (sec) the cooldown time for republishing published data, please make sure that it's greater than the ego's stopping time.
+
         super().__init__(args, "perception_reproducer_v2")
-        
-        self.reproduce_sequence_indices = deque()# contains ego_odom_idx
-        self.cooldown_indices = deque() # contains ego_odom_idx
-        self.ego_odom_id2last_published_timestamp=dict() # for checking last published timestap.
+
+        self.reproduce_sequence_indices = deque()  # contains ego_odom_idx
+        self.cooldown_indices = deque()  # contains ego_odom_idx
+        self.ego_odom_id2last_published_timestamp = dict()  # for checking last published timestap.
 
         self.prev_ego_pos = None
         self.prev_ego_odom_msg = None
         self.perv_objects_msg = None
         self.prev_traffic_signals_msg = None
 
-
         self.stopwatch = StopWatch(self.args.verbose)  # for debug
 
         # to make some data to accelerate computation
@@ -56,10 +61,10 @@ def __init__(self, args):
         time_diffs = []
         prev_stamp = self.rosbag_ego_odom_data[0][0]
         for stamp, _ in self.rosbag_ego_odom_data[1:]:
-            time_diff = (stamp - prev_stamp)/ 1e9
+            time_diff = (stamp - prev_stamp) / 1e9
             time_diffs.append(time_diff)
             prev_stamp = stamp
-        
+
         average_ego_odom_interval = sum(time_diffs) / len(time_diffs)
         self.timer = self.create_timer(average_ego_odom_interval, self.on_timer)
 
@@ -95,53 +100,57 @@ def on_timer(self):
         if not self.ego_pose:
             print("No ego pose found.")
             return
-
-        ## Update reproduce list if ego_pos is moved.
-        if self.ego_pose is None \
-                or self.prev_ego_pos is None or \
-                ((self.ego_pose.position.x - self.prev_ego_pos.position.x)**2 + \
-                 (self.ego_pose.position.y - self.prev_ego_pos.position.y)**2) > dist_eps**2:
 
+        ## Update reproduce list if ego_pos is moved.
+        if (
+            self.ego_pose is None
+            or self.prev_ego_pos is None
+            or (
+                (self.ego_pose.position.x - self.prev_ego_pos.position.x) ** 2
+                + (self.ego_pose.position.y - self.prev_ego_pos.position.y) ** 2
+            )
+            > dist_eps**2
+        ):
             # find the nearest ego odom by simulation observation
             self.stopwatch.tic("find_nearest_ego_odom_by_observation")
             nearst_ego_odom_ind = self.find_nearest_ego_odom_index(self.ego_pose)
             nearst_ego_odom_pos = self.rosbag_ego_odom_data[nearst_ego_odom_ind][1].pose.pose
             self.stopwatch.toc("find_nearest_ego_odom_by_observation")
 
-            # find a list of ego odoms around the nearst_ego_odom_pos. 
+            # find a list of ego odoms around the nearst_ego_odom_pos.
             self.stopwatch.tic("find_nearby_ego_odom_indies")
-            ego_odom_indices = self.find_nearby_ego_odom_indies(nearst_ego_odom_pos, 
-                                                                self.rosbag_ego_odom_seach_radius)
+            ego_odom_indices = self.find_nearby_ego_odom_indies(
+                nearst_ego_odom_pos, self.rosbag_ego_odom_seach_radius
+            )
             self.stopwatch.toc("find_nearby_ego_odom_indies")
 
             # update reproduce_sequence with those data not in cooldown list.
             while self.cooldown_indices:
                 last_timestamp = self.ego_odom_id2last_published_timestamp[self.cooldown_indices[0]]
-                if (timestamp - last_timestamp).nanoseconds/1e9 > self.reproduce_cooldown:
+                if (timestamp - last_timestamp).nanoseconds / 1e9 > self.reproduce_cooldown:
                     self.cooldown_indices.popleft()
                 else:
                     break
 
-
             self.stopwatch.tic("update reproduce_sequence")
             self.reproduce_sequence_indices = deque(
-                [idx for idx in ego_odom_indices 
-                 if idx not in self.cooldown_indices])
+                [idx for idx in ego_odom_indices if idx not in self.cooldown_indices]
+            )
             self.stopwatch.toc("update reproduce_sequence")
 
         self.prev_ego_pos = self.ego_pose
-        
+
         ## get data to publish
         repeat_trigger = len(self.reproduce_sequence_indices) == 0
-        if not repeat_trigger:# pop data from reproduce_sequence if sequence is not empty.
+        if not repeat_trigger:  # pop data from reproduce_sequence if sequence is not empty.
             ego_odom_idx = self.reproduce_sequence_indices.popleft()
             pose_timestamp, ego_odom_msg = self.rosbag_ego_odom_data[ego_odom_idx]
             # extract message by the nearest ego odom timestamp
             self.stopwatch.tic("find_topics_by_timestamp")
             objects_msg, traffic_signals_msg = self.find_topics_by_timestamp(pose_timestamp)
             self.stopwatch.toc("find_topics_by_timestamp")
 
-        else:#get perv data to publish if reproduce_sequence is empty.
+        else:  # get perv data to publish if reproduce_sequence is empty.
             ego_odom_msg = self.prev_ego_odom_msg
             objects_msg = self.perv_objects_msg
             traffic_signals_msg = self.prev_traffic_signals_msg
@@ -158,7 +167,9 @@ def on_timer(self):
             if self.args.detected_object:
                 # copy the messages
                 self.stopwatch.tic("message deepcopy")
-                objects_msg_copied = pickle.loads(pickle.dumps(objects_msg))  # this is x5 faster than deepcopy
+                objects_msg_copied = pickle.loads(
+                    pickle.dumps(objects_msg)
+                )  # this is x5 faster than deepcopy
                 self.stopwatch.toc("message deepcopy")
 
                 log_ego_pose = ego_odom_msg.pose.pose
@@ -167,7 +178,6 @@ def on_timer(self):
                 objects_msg_copied = objects_msg
             self.objects_pub.publish(objects_msg_copied)
 
-
         # traffic signals
         if traffic_signals_msg:
             traffic_signals_msg.stamp = timestamp_msg
@@ -181,8 +191,14 @@ def on_timer(self):
         if not repeat_trigger:
             # save data for repeate pubulication.
             self.prev_ego_odom_msg = ego_odom_msg
-            self.perv_objects_msg = objects_msg if objects_msg is not None else self.perv_objects_msg
-            self.prev_traffic_signals_msg = traffic_signals_msg if traffic_signals_msg is not None else self.prev_traffic_signals_msg
+            self.perv_objects_msg = (
+                objects_msg if objects_msg is not None else self.perv_objects_msg
+            )
+            self.prev_traffic_signals_msg = (
+                traffic_signals_msg
+                if traffic_signals_msg is not None
+                else self.prev_traffic_signals_msg
+            )
 
             # update cooldown info.
             self.ego_odom_id2last_published_timestamp[ego_odom_idx] = timestamp
@@ -196,18 +212,20 @@ def find_nearest_ego_odom_index(self, ego_pose):
         dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1)
         nearest_idx = np.argmin(dists_squared)
         return nearest_idx
-    
+
     def find_nearby_ego_odom_indies(self, ego_pose, search_radius: float):
-        '''
+        """
         Find the indices of the ego odoms that are within a certain distance of the ego_pose.
         Inputs:
             ego_pose: the ego pose to search around.
-            search_radius: the radius to search around the ego_pose.'''
+            search_radius: the radius to search around the ego_pose."""
         self_pose = np.array([ego_pose.position.x, ego_pose.position.y])
         dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1)
         nearby_indices = np.where(dists_squared <= search_radius**2)[0]
 
         return nearby_indices
+
+
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
     parser.add_argument("-b", "--bag", help="rosbag", default=None)
@@ -224,10 +242,18 @@ def find_nearby_ego_odom_indies(self, ego_pose, search_radius: float):
         "-v", "--verbose", help="output debug data", action="store_true", default=False
     )
     parser.add_argument(
-        "-r", "--seach-radius", help="the search radius for searching rosbag's ego odom messages around the nearest ego odom pose (default is 1 meters)", type=float, default=1.0
+        "-r",
+        "--seach-radius",
+        help="the search radius for searching rosbag's ego odom messages around the nearest ego odom pose (default is 1 meters)",
+        type=float,
+        default=1.0,
     )
     parser.add_argument(
-        "-c", "--reproduce-cooldown", help="The cooldown time for republishing published messages (default is 40.0 seconds)", type=float, default=40.0
+        "-c",
+        "--reproduce-cooldown",
+        help="The cooldown time for republishing published messages (default is 40.0 seconds)",
+        type=float,
+        default=40.0,
     )
     args = parser.parse_args()
 

planning/planning_debug_tools/scripts/perception_replayer/utils.py

@@ -25,10 +25,12 @@
 from tf_transformations import euler_from_quaternion
 from tf_transformations import quaternion_from_euler
 
+
 def get_starting_time(uri: str):
     info = rosbag2_py.Info().read_metadata(uri, "sqlite3")
     return info.starting_time
 
+
 def get_rosbag_options(path, serialization_format="cdr"):
     storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3")