Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Feb 24, 2025
1 parent 65282e1 commit 5ae7f78
Show file tree
Hide file tree
Showing 5 changed files with 328 additions and 242 deletions.
2 changes: 1 addition & 1 deletion map/autoware_tp_collector/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
<buildtool_depend>tier4_debug_msgs</buildtool_depend>

<depend>libpcl-all-dev</depend>
<depend>yaml-cpp</depend>
<depend>tier4_debug_msgs</depend>
<depend>yaml-cpp</depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
168 changes: 100 additions & 68 deletions map/autoware_tp_collector/scripts/tp_checker.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,35 +14,34 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import os
import argparse
from rosbag2_py import (
SequentialReader,
StorageOptions,
ConverterOptions,
Info
)

from rclpy.serialization import deserialize_message, serialize_message
import os
import csv
import yaml
from scipy import spatial as sp
import numpy as np
import os
import shutil
import struct
import time

from builtin_interfaces.msg import Time
from geometry_msgs.msg import PoseWithCovarianceStamped
import sensor_msgs.msg as sensor_msgs
import std_msgs.msg as std_msgs
import numpy as np
import open3d as o3d
import rclpy
from rclpy.node import Node
from tier4_debug_msgs.msg import Float32Stamped
from builtin_interfaces.msg import Time
import tqdm
import time
import shutil
import open3d as o3d
from rclpy.serialization import deserialize_message
from rclpy.serialization import serialize_message
from rosbag2_py import ConverterOptions
from rosbag2_py import Info
from rosbag2_py import SequentialReader
from rosbag2_py import StorageOptions
from scipy import spatial as sp
import sensor_msgs.msg as sensor_msgs
import sensor_msgs_py.point_cloud2 as pc2
import struct
import std_msgs.msg as std_msgs
from tier4_debug_msgs.msg import Float32Stamped
import tp_utility as tpu
import tqdm
import yaml


def mark_changes(candidate_segments, rosbag_tp: float, segment_dict, segment_df):
tp_sum = 0.0
Expand All @@ -57,21 +56,23 @@ def mark_changes(candidate_segments, rosbag_tp: float, segment_dict, segment_df)
else:
expected_tp = 0

if (expected_tp > 0 and abs(expected_tp - rosbag_tp) / expected_tp >= 0.2) or \
(expected_tp == 0 and rosbag_tp > 0):
if (expected_tp > 0 and abs(expected_tp - rosbag_tp) / expected_tp >= 0.2) or (
expected_tp == 0 and rosbag_tp > 0
):
for key in candidate_segments:
if key in segment_dict:
segment_df[segment_dict[key], 2] += 1


class TPChecker(Node):

def __init__(self):
super().__init__('tp_checker')
super().__init__("tp_checker")
self.segment_df = None # Segment indices, TP
self.segment_dict = {} # key: segment name, value: index to the segment_df
self.pcd_path = None # Path to the directory containing PCD segments
self.changed_dir = None # A directory contains the segments that need to be examined
self.result_csv = None # Path to the result CSV file
self.pcd_path = None # Path to the directory containing PCD segments
self.changed_dir = None # A directory contains the segments that need to be examined
self.result_csv = None # Path to the result CSV file
self.tp_path = None # Path to the file that contains TPs of map segments

def __initialize(self, pcd_map_dir: str):
Expand All @@ -94,7 +95,11 @@ def __initialize(self, pcd_map_dir: str):
self.tp_path = os.path.join(pcd_map_dir, "scores.csv")

if not os.path.exists(self.tp_path):
print("Error: A TP file, which contains the TPs of map segments, is not found at {0}! Abort!".format(self.tp_path))
print(
"Error: A TP file, which contains the TPs of map segments, is not found at {0}! Abort!".format(
self.tp_path
)
)
exit()

# Read the input map directory and setup the segment dictionary
Expand All @@ -112,8 +117,8 @@ def __get_pcd_segments_and_scores(self):
elif key == "x_resolution":
self.resolution = value

self.segment_df = np.array(self.segment_df, dtype = object)
self.segment_df = np.array(self.segment_df, dtype=object)

# Load the TPs
with open(self.tp_path, "r") as f:
reader = csv.reader(f)
Expand All @@ -136,15 +141,21 @@ def __show(self):
dtype = np.float32
itemsize = np.dtype(dtype).itemsize

fields = [sensor_msgs.PointField(name = "x", offset = 0, datatype = ros_float_dtype, count = 1),
sensor_msgs.PointField(name = "y", offset = itemsize, datatype = ros_float_dtype, count = 1),
sensor_msgs.PointField(name = "z", offset = itemsize * 2, datatype = ros_float_dtype, count = 1),
sensor_msgs.PointField(name = "rgba", offset = itemsize * 3, datatype = ros_uint32_dtype, count = 1)]
fields = [
sensor_msgs.PointField(name="x", offset=0, datatype=ros_float_dtype, count=1),
sensor_msgs.PointField(name="y", offset=itemsize, datatype=ros_float_dtype, count=1),
sensor_msgs.PointField(
name="z", offset=itemsize * 2, datatype=ros_float_dtype, count=1
),
sensor_msgs.PointField(
name="rgba", offset=itemsize * 3, datatype=ros_uint32_dtype, count=1
),
]

points = []
pc2_width = 0

progress_bar = tqdm.tqdm(total = len(self.segment_df))
progress_bar = tqdm.tqdm(total=len(self.segment_df))
origin = None

for i in range(self.segment_df.shape[0]):
Expand All @@ -167,65 +178,84 @@ def __show(self):
header.frame_id = "map"

pc2_msg = pc2.create_cloud(header, fields, points)
pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, '/autoware_tp_checker', 10)
pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, "/autoware_tp_checker", 10)

while True:
pcd_publisher.publish(pc2_msg)
time.sleep(5)


def __set_color_based_on_mark(self, mark) -> int:
# The may-be-changed segments are colored red
# The may-not-be-changed segments are colored white
if mark >= 100:
r = 255
g = 0
b = 0
b = 0
else:
r = 255
g = 255
b = 255
a = 255

tmp_rgb = struct.pack('BBBB', b, g, r, a)
rgba = struct.unpack('I', tmp_rgb)[0]
tmp_rgb = struct.pack("BBBB", b, g, r, a)
rgba = struct.unpack("I", tmp_rgb)[0]

return rgba

def processing(self,
pcd_path: str,
rosbag_path: str,
result_path: str,
pose_topic: str,
tp_topic: str,
scan_topic: str):
def processing(
self,
pcd_path: str,
rosbag_path: str,
result_path: str,
pose_topic: str,
tp_topic: str,
scan_topic: str,
):
self.__initialize(pcd_path)
self.__get_pcd_segments_and_scores()

tpu.collect_rosbag_tp(rosbag_path, pose_topic, tp_topic, scan_topic, self.resolution,
mark_changes, self.segment_dict, self.segment_df)
tpu.collect_rosbag_tp(
rosbag_path,
pose_topic,
tp_topic,
scan_topic,
self.resolution,
mark_changes,
self.segment_dict,
self.segment_df,
)

self.__save_results()
self.__show()


if __name__ == "__main__":
rclpy.init()
parser = argparse.ArgumentParser()
parser.add_argument("map_path", help="The path to the PCD folder")
parser.add_argument("bag_path", help="The path to the input rosbag")
parser.add_argument("result_path", help="The path to the result folder")
parser.add_argument("--pose_topic",
help = "Pose topic",
default = "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias",
required = False, type = str)
parser.add_argument("--tp_topic",
help = "TP topic",
default = "/localization/pose_estimator/transform_probability",
required = False, type = str)
parser.add_argument("--scan_topic",
help = "Point cloud topic",
default = "/localization/util/downsample/pointcloud",
required = False, type = str)
parser.add_argument(
"--pose_topic",
help="Pose topic",
default="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias",
required=False,
type=str,
)
parser.add_argument(
"--tp_topic",
help="TP topic",
default="/localization/pose_estimator/transform_probability",
required=False,
type=str,
)
parser.add_argument(
"--scan_topic",
help="Point cloud topic",
default="/localization/util/downsample/pointcloud",
required=False,
type=str,
)

args = parser.parse_args()

Expand All @@ -241,9 +271,11 @@ def processing(self,
checker = TPChecker()

# checker.processing(args.map_path, args.bag_path, args.result_path)
checker.processing(args.map_path,
args.bag_path,
args.result_path,
args.pose_topic,
args.tp_topic,
args.scan_topic)
checker.processing(
args.map_path,
args.bag_path,
args.result_path,
args.pose_topic,
args.tp_topic,
args.scan_topic,
)
Loading

0 comments on commit 5ae7f78

Please sign in to comment.