diff --git a/map/autoware_tp_collector/README.md b/map/autoware_tp_collector/README.md deleted file mode 100644 index 2197dda94..000000000 --- a/map/autoware_tp_collector/README.md +++ /dev/null @@ -1,48 +0,0 @@ -# autoware_pointcloud_merger - -This is a tool for processing pcd files, and it can perform the following functions: - -- Merging multiple PCD files to a single PCD file -- Downsampling point clouds - -## Supported Data Format - -**Currently, only `pcl::PointXYZ` and `pcl::PointXYZI` are supported. Any PCD will be loaded as those two types .** - -This tool can be used with files that have data fields other than `XYZI` (e.g., `XYZRGB`) and files that only contain `XYZ`. - -- Data fields other than `XYZI` are ignored during loading. -- When loading `XYZ`-only data, the `intensity` field is assigned 0. - -## Installation - -```bash -cd # OR -cd src/ -git clone git@github.com:autowarefoundation/autoware_tools.git -cd .. -colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests --symlink-install --packages-up-to autoware_pointcloud_merger -``` - -## Usage - -- Merger all PCD files from the input directory into a single output PCD - - ```bash - ros2 launch autoware_pointcloud_merger pointcloud_merger.launch.xml input_pcd_dir:= output_pcd:= - ``` - - | Name | Description | - | ---------- | ------------------------------------------- | - | INPUT_DIR | Directory that contains all input PCD files | - | OUTPUT_PCD | Name of the output PCD file | - -`INPUT_DIR` and `OUTPUT_PCD` should be specified as **absolute paths**. - -## Parameter - -{{ json_to_markdown("map/autoware_pointcloud_merger/schema/pointcloud_merger.schema.json") }} - -## LICENSE - -Parts of files pcd_merger.hpp, and pcd_merger.cpp are copied from [MapIV's pointcloud_divider](https://github.com/MapIV/pointcloud_divider) and are under [BSD-3-Clauses](LICENSE) license. The remaining code are under [Apache License 2.0](../../LICENSE) diff --git a/map/autoware_tp_collector/schema/pointcloud_merger.schema.json b/map/autoware_tp_collector/schema/pointcloud_merger.schema.json deleted file mode 100644 index 21311d69a..000000000 --- a/map/autoware_tp_collector/schema/pointcloud_merger.schema.json +++ /dev/null @@ -1,48 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for autoware point cloud merger node", - "type": "object", - "definitions": { - "autoware_pointcloud_merger": { - "type": "object", - "properties": { - "leaf_size": { - "type": "number", - "description": "Resolution in meter for downsampling the output PCD. Setting to negative to get the raw output PCD.", - "default": "-0.1" - }, - "input_pcd_dir": { - "type": "string", - "description": "The path to the folder containing the input PCD files", - "default": "" - }, - "output_pcd": { - "type": "string", - "description": "The path to the merged PCD file", - "default": "" - }, - "point_type": { - "type": "string", - "description": "Type of the point when processing PCD files. Could be point_xyz or point_xyzi", - "default": "point_xyzi" - } - }, - "required": ["input_pcd_dir", "output_pcd"], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/autoware_pointcloud_merger" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} diff --git a/map/autoware_tp_collector/CMakeLists.txt b/map/autoware_tp_manager/CMakeLists.txt similarity index 70% rename from map/autoware_tp_collector/CMakeLists.txt rename to map/autoware_tp_manager/CMakeLists.txt index 45de9535d..58ffcf92d 100644 --- a/map/autoware_tp_collector/CMakeLists.txt +++ b/map/autoware_tp_manager/CMakeLists.txt @@ -4,13 +4,6 @@ project(autoware_tp_manager) find_package(autoware_cmake REQUIRED) autoware_package() -# Enable support for C++17 -if (${CMAKE_VERSION} VERSION_LESS "3.1.0") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") -else () - set(CMAKE_CXX_STANDARD 17) -endif () - # Find packages find_package(yaml-cpp REQUIRED) find_package(PCL REQUIRED) diff --git a/map/autoware_tp_collector/LICENSE b/map/autoware_tp_manager/LICENSE similarity index 100% rename from map/autoware_tp_collector/LICENSE rename to map/autoware_tp_manager/LICENSE diff --git a/map/autoware_tp_manager/README.md b/map/autoware_tp_manager/README.md new file mode 100644 index 000000000..8dd928113 --- /dev/null +++ b/map/autoware_tp_manager/README.md @@ -0,0 +1,78 @@ +# autoware_tp_manager + +Here are some tools for collecting average TPs of PCD maps. Currently, we consider the decrease of TPs as a sign of map decay. However, we don't know what TPs are 'abnormal', e.g. in some areas the TPs range around 2.0 ~ 3.0, while in others TPs float around 5.0. This package provides some tools to check it, including: +- TP collector: collect the average TPs per segment of a PCD map +- TP checker: compare a rosbag's TPs with a map's TPs and highlight the map areas where the rosbag's TPs differ significantly from the map's TPs. + +## Installation + +```bash +cd # OR +cd src/ +git clone git@github.com:autowarefoundation/autoware_tools.git +cd .. +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests --symlink-install --packages-up-to autoware_pointcloud_merger +``` + +## Usage + +- Collect the average TPs per segment from a map by TP_collector + + ```bash + ros2 run autoware_tp_manager tp_collector.py [--resolution:=] [--pose_topic:=] [--tp_topic:=] [--scan_topic:=] + ``` + + | Name | Description | + | ----------------- | ------------------------------------------- | + | path_to_pcd_dir | Directory that contains the input PCD files | + | path_to_rosbag | Path to the input rosbag | + | path_to_output_dir| Path to the output directory | + | resolution | Resolution to segment the input PCD. The TPs are collected on these segments| + | topic_of_poses | Topic of poses messages in the input rosbag| + | topic_of_TPs | Topic of TPs in the input rosbag| + | topic_of_scans | Topic of downsampled scans in the input rosbag| + + Paths to folders should be specified as **absolute paths**. + + The rosbag should contain the following topics + - /localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias + - /localization/pose_estimator/transform_probability + - /localization/util/downsample/pointcloud + + The average TPs can be visualized on Rviz2 by running the following command + ```bash + python3 install/autoware_tp_manager/lib/autoware_tp_manager/tp_visualizer.py + ``` + | Name | Description | + | ----------------- | ------------------------------------------- | + | path_to_output_dir| Path to the output directory of TP_collector| + + then open another terminal to launch Rviz2 and add the topic /autoware_tp_visualizer. + +- Compare a rosbags' TPs with a map's TPs by TP_checker + ```bash + ros2 run autoware_tp_manager tp_checker.py [--pose_topic:=] [--tp_topic:=] [--scan_topic:=] + ``` + + | Name | Description | + | ----------------- | ------------------------------------------- | + | path_to_score_dir | Directory that contains the TP file (.csv) and the downsampled PCD map. This is the output directory of the tp_collector. | + | path_to_rosbag | Path to the input rosbag to be evaluated| + | topic_of_poses | Topic of poses in the evaluation rosbag | + | topic_of_TPs | Topic of TPs in the evaluation rosbag | + | topic_of_scans | Topic of scans in the evaluation rosbag | + + The results of checking are published to the topic /autoware_tp_checker, and can also be displayed on Rviz2. The red points + +- The rosbags used to both TP collector and TP checker is created by running Autoware's logging simulator and record the following three topics: + - /localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias + - /localization/pose_estimator/transform_probability + - /localization/util/downsample/pointcloud + +## Parameter + +{{ json_to_markdown("map/autoware_tp_manager/schema/tp_manager.schema.json") }} + +## LICENSE + +This package is under [Apache License 2.0](../../LICENSE) diff --git a/map/autoware_tp_collector/package.xml b/map/autoware_tp_manager/package.xml similarity index 100% rename from map/autoware_tp_collector/package.xml rename to map/autoware_tp_manager/package.xml index 611caeebb..7e9bb440d 100644 --- a/map/autoware_tp_collector/package.xml +++ b/map/autoware_tp_manager/package.xml @@ -14,8 +14,8 @@ tier4_debug_msgs libpcl-all-dev - tier4_debug_msgs yaml-cpp + tier4_debug_msgs ament_cmake diff --git a/map/autoware_tp_manager/schema/tp_manager.schema.json b/map/autoware_tp_manager/schema/tp_manager.schema.json new file mode 100644 index 000000000..0af1e40b3 --- /dev/null +++ b/map/autoware_tp_manager/schema/tp_manager.schema.json @@ -0,0 +1,107 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for autoware tp manager node", + "type": "object", + "definitions": { + "tp_collector": { + "type": "object", + "properties": { + "path_to_pcd_dir": { + "type": "string", + "description": "The path to the directory containing the input PCD files", + "default": "" + }, + "path_to_rosbag": { + "type": "string", + "description": "The path to the input rosbag", + "default": "" + }, + "path_to_output_dir": { + "type": "string", + "description": "The path to the output directory", + "default": "" + }, + "resolution": { + "type": "number", + "description": "Resolution to segment the input PCD. The TPs are collected on these segments.", + "default": "5,0" + }, + "pose_topic": { + "type": "string", + "description": "Topic of poses messages in the input rosbag", + "default": "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias" + }, + "tp_topic": { + "type": "string", + "description": "Topic of TPs in the input rosbag", + "default": "/localization/pose_estimator/transform_probability" + }, + "scan_topic": { + "type": "string", + "description": "Topic of downsampled scans in the input rosbag", + "default": "/localization/util/downsample/pointcloud" + } + }, + "required": ["path_to_pcd_dir", "path_to_rosbag", "path_to_output_dir"], + "additionalProperties": false + }, + "tp_checker": { + "type": "object", + "properties": { + "path_to_score_dir": { + "type": "string", + "description": "The path to the directory containing the average TP file (.csv) and the downsampled PCD map. This is also the output directory of the TP collector.", + "default": "" + }, + "path_to_rosbag": { + "type": "string", + "description": "Path to the input rosbag to be evaluated", + "default": "" + }, + "pose_topic": { + "type": "string", + "description": "Topic of poses messages in the input rosbag", + "default": "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias" + }, + "tp_topic": { + "type": "string", + "description": "Topic of TPs in the input rosbag", + "default": "/localization/pose_estimator/transform_probability" + }, + "scan_topic": { + "type": "string", + "description": "Topic of downsampled scans in the input rosbag", + "default": "/localization/util/downsample/pointcloud" + } + }, + "required": ["path_to_score_dir", "path_to_rosbag"], + "additionalProperties": false + }, + "tp_visualizer": { + "type": "object", + "properties": { + "path_to_output_dir": { + "type": "string", + "description": "Path to the output directory of TP_collector", + "default": "" + } + }, + "required": ["path_to_output_dir"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/autoware_pointcloud_merger" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/autoware_tp_collector/scripts/tp_checker.py b/map/autoware_tp_manager/scripts/tp_checker.py similarity index 58% rename from map/autoware_tp_collector/scripts/tp_checker.py rename to map/autoware_tp_manager/scripts/tp_checker.py index a86038e71..08aa33099 100755 --- a/map/autoware_tp_collector/scripts/tp_checker.py +++ b/map/autoware_tp_manager/scripts/tp_checker.py @@ -14,34 +14,35 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os import argparse -import csv +from rosbag2_py import ( + SequentialReader, + StorageOptions, + ConverterOptions, + Info +) + +from rclpy.serialization import deserialize_message, serialize_message import os -import shutil -import struct -import time - -from builtin_interfaces.msg import Time -from geometry_msgs.msg import PoseWithCovarianceStamped -import numpy as np -import open3d as o3d -import rclpy -from rclpy.node import Node -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 +import csv +import yaml from scipy import spatial as sp +import numpy as np +from geometry_msgs.msg import PoseWithCovarianceStamped import sensor_msgs.msg as sensor_msgs -import sensor_msgs_py.point_cloud2 as pc2 import std_msgs.msg as std_msgs +import rclpy +from rclpy.node import Node from tier4_debug_msgs.msg import Float32Stamped -import tp_utility as tpu +from builtin_interfaces.msg import Time import tqdm -import yaml - +import time +import shutil +import open3d as o3d +import sensor_msgs_py.point_cloud2 as pc2 +import struct +import tp_utility as tpu def mark_changes(candidate_segments, rosbag_tp: float, segment_dict, segment_df): tp_sum = 0.0 @@ -56,50 +57,44 @@ 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): - if not os.path.exists(pcd_map_dir): - print("Error: {0} does not exist! Abort!".format(pcd_map_dir)) + def __initialize(self, score_dir: str): + if not os.path.exists(score_dir): + print("Error: {0} does not exist! Abort!".format(score_dir)) exit() - self.pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd/") + self.pcd_path = os.path.join(score_dir, "pointcloud_map.pcd") if not os.path.exists(self.pcd_path): print("Error: {0} does not exist! Abort!".format(self.pcd_path)) exit() - self.yaml_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") + self.yaml_path = os.path.join(score_dir, "pointcloud_map_metadata.yaml") if not os.path.exists(self.yaml_path): print("Error: A map metadata file is not found at {0}! Abort!".format(self.yaml_path)) exit() - self.tp_path = os.path.join(pcd_map_dir, "scores.csv") + self.tp_path = os.path.join(score_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 @@ -117,8 +112,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) @@ -129,33 +124,21 @@ def __get_pcd_segments_and_scores(self): for index, row in enumerate(reader): self.segment_df[index, 1] = float(row[1]) - # Save the tp and average tps to CSV file ##### - def __save_results(self: str): - pass - # self.ndt_res_df.to_csv(self.result_csv) - # print("The checking results are saved at {0}".format(self.result_csv)) - def __show(self): ros_float_dtype = sensor_msgs.PointField.FLOAT32 ros_uint32_dtype = sensor_msgs.PointField.UINT32 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]): @@ -178,91 +161,68 @@ 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 + # The may-have-changed segments are colored red + # The may-not-have-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, - ): - self.__initialize(pcd_path) + def processing(self, + score_path: str, + rosbag_path: str, + pose_topic: str, + tp_topic: str, + scan_topic: str): + self.__initialize(score_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, - ) - - self.__save_results() - self.__show() + tpu.collect_rosbag_tp(rosbag_path, pose_topic, tp_topic, scan_topic, self.resolution, + mark_changes, self.segment_dict, self.segment_df) + 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("score_path", help="The path to the folder containing the TP file") 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() # Practice with string % a bit - print("Input PCD map at {0}".format(args.map_path)) + print("Input PCD map at {0}".format(args.score_path)) print("Input rosbag at {0}".format(args.bag_path)) - print("Results are saved at {0}".format(args.result_path)) print("Topic of NDT poses {0}".format(args.pose_topic)) print("Topic of Transformation Probability {0}".format(args.tp_topic)) print("Topic of scan data {0}".format(args.scan_topic)) @@ -270,12 +230,8 @@ def processing( # Run 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.score_path, + args.bag_path, + args.pose_topic, + args.tp_topic, + args.scan_topic) diff --git a/map/autoware_tp_collector/scripts/tp_collector.py b/map/autoware_tp_manager/scripts/tp_collector.py similarity index 65% rename from map/autoware_tp_collector/scripts/tp_collector.py rename to map/autoware_tp_manager/scripts/tp_collector.py index 9b38e5c41..4186edc47 100755 --- a/map/autoware_tp_collector/scripts/tp_collector.py +++ b/map/autoware_tp_manager/scripts/tp_collector.py @@ -14,32 +14,32 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os import argparse -import csv +from rosbag2_py import ( + SequentialReader, + StorageOptions, + ConverterOptions, + Info +) + +from rclpy.serialization import deserialize_message, serialize_message import os -import shutil -from subprocess import call - -from builtin_interfaces.msg import Time -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseWithCovarianceStamped -import numpy as np -import rclpy -from rclpy.node import Node -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 +import csv +import yaml from scipy import spatial as sp +import numpy as np +from geometry_msgs.msg import PoseWithCovarianceStamped, Pose +from tier4_debug_msgs.msg import Float32Stamped +from builtin_interfaces.msg import Time +import tqdm import sensor_msgs.msg as sensor_msgs import sensor_msgs_py.point_cloud2 as pc2 -from tier4_debug_msgs.msg import Float32Stamped +import rclpy +from rclpy.node import Node +import shutil +from subprocess import call import tp_utility as tpu -import tqdm -import yaml - # Update the TP of a segment at index @idx, given a newly TP value def update_avg_tp(candidate_segments, new_tp, segment_dict, segment_df): @@ -49,15 +49,14 @@ def update_avg_tp(candidate_segments, new_tp, segment_dict, segment_df): tp, counter = segment_df[i, [1, 2]] segment_df[i, [1, 2]] = [tp + 1.0 / (counter + 1) * (new_tp - tp), counter + 1] - class TPCollector(Node): def __init__(self): - super().__init__("tp_collector") + super().__init__('tp_collector') self.pcd_path = None self.yaml_path = None self.score_path = None self.segment_df = None - self.segment_dict = {} # Pairs of 2D coordinate and index + self.segment_dict = {} # Pairs of 2D coordinate and index self.resolution = None def __initialize(self, pcd_map_dir: str, output_path: str, resolution: float): @@ -94,19 +93,11 @@ def __get_pcd_segments_and_scores(self): self.yaml_path = os.path.join(self.output_path, "pointcloud_map_metadata.yaml") if not os.path.exists(self.yaml_path): - ds_cmd = ( - "ros2 launch autoware_pointcloud_divider pointcloud_divider.launch.xml " - + "input_pcd_or_dir:=" - + self.pcd_path - + " output_pcd_dir:=" - + self.output_path - + " prefix:=test leaf_size:=0.5" - + " grid_size_x:=" - + str(self.resolution) - + " grid_size_y:=" - + str(self.resolution) - ) - call(ds_cmd, shell=True) + ds_cmd = "ros2 launch autoware_pointcloud_divider pointcloud_divider.launch.xml " + \ + "input_pcd_or_dir:=" + self.pcd_path + " output_pcd_dir:=" + self.output_path + \ + " prefix:=test leaf_size:=0.5" + " grid_size_x:=" + str(self.resolution) + \ + " grid_size_y:=" + str(self.resolution) + call(ds_cmd, shell = True) self.pcd_path = os.path.join(self.output_path, "pointcloud_map.pcd") # Now scan the downsample directory and get the segment list @@ -117,7 +108,7 @@ def __get_pcd_segments_and_scores(self): seg_key = str(value[0]) + "_" + str(value[1]) self.segment_dict[seg_key] = len(self.segment_df) - 1 - self.segment_df = np.array(self.segment_df, dtype=object) + self.segment_df = np.array(self.segment_df, dtype = object) # Load the score map self.score_path = os.path.join(self.output_path, "scores.csv") @@ -128,30 +119,29 @@ def __get_pcd_segments_and_scores(self): # Skip the header next(reader) - # Load the current maps' TPs + # Load the current maps' TPs for index, row in enumerate(reader): self.segment_df[index, [1, 2]] = [float(row[1]), 1] - + ##### Save the segment TPs ##### def __save_tps(self): print("Saving TP to files") with open(self.score_path, "w") as f: f.write("segment,tp\n") print("Number of segments = {0}".format(self.segment_df.shape[0])) - for i in np.arange(0, self.segment_df.shape[0], dtype=int): + for i in np.arange(0, self.segment_df.shape[0], dtype = int): f.write("{0},{1}\n".format(self.segment_df[i, 0], self.segment_df[i, 1])) print("Done. Segments' TPs are saved at {0}.".format(self.score_path)) - def processing( - self, - pcd_map_dir: str, - rosbag_path: str, - output_path: str, - resolution: float, - pose_topic: str, - tp_topic: str, - scan_topic: str, - ): + + def processing(self, + pcd_map_dir: str, + rosbag_path: str, + output_path: str, + resolution: float, + pose_topic: str, + tp_topic: str, + scan_topic: str): # Initialize paths to directories self.__initialize(pcd_map_dir, output_path, resolution) @@ -159,51 +149,32 @@ def processing( self.__get_pcd_segments_and_scores() # Read the rosbag and get the ndt poses and corresponding tps - tpu.collect_rosbag_tp( - rosbag_path, - pose_topic, - tp_topic, - scan_topic, - self.resolution, - update_avg_tp, - self.segment_dict, - self.segment_df, - ) + tpu.collect_rosbag_tp(rosbag_path, pose_topic, tp_topic, scan_topic, + self.resolution, update_avg_tp, + self.segment_dict, self.segment_df) # Save the new TPs self.__save_tps() - if __name__ == "__main__": rclpy.init() parser = argparse.ArgumentParser() - parser.add_argument("map_path", help="The path to the PCD folder", type=str) - parser.add_argument("bag_path", help="The path to the input rosbag", type=str) - parser.add_argument("output", help="The path to the output directory", type=str) - parser.add_argument( - "--resolution", help="Map segment resolution", default=20, required=False, type=float - ) - 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("map_path", help = "The path to the PCD folder", type = str) + parser.add_argument("bag_path", help = "The path to the input rosbag", type = str) + parser.add_argument("output", help = "The path to the output directory", type = str) + parser.add_argument("--resolution", help = "Map segment resolution", default = 20, required = False, type = float) + 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() @@ -218,12 +189,10 @@ def processing( # Run tp_collector = TPCollector() - tp_collector.processing( - args.map_path, - args.bag_path, - args.output, - args.resolution, - args.pose_topic, - args.tp_topic, - args.scan_topic, - ) + tp_collector.processing(args.map_path, + args.bag_path, + args.output, + args.resolution, + args.pose_topic, + args.tp_topic, + args.scan_topic) diff --git a/map/autoware_tp_collector/scripts/tp_utility.py b/map/autoware_tp_manager/scripts/tp_utility.py similarity index 62% rename from map/autoware_tp_collector/scripts/tp_utility.py rename to map/autoware_tp_manager/scripts/tp_utility.py index fa3beb4d0..128f43a4a 100644 --- a/map/autoware_tp_collector/scripts/tp_utility.py +++ b/map/autoware_tp_manager/scripts/tp_utility.py @@ -1,27 +1,42 @@ -from geometry_msgs.msg import Point -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseWithCovarianceStamped -from geometry_msgs.msg import Quaternion -import numpy as np -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 -import sensor_msgs.msg as sensor_msgs -import sensor_msgs_py.point_cloud2 as pc2 +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from rosbag2_py import ( + SequentialReader, + StorageOptions, + ConverterOptions, + Info +) +from rclpy.serialization import deserialize_message, serialize_message +from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion from tier4_debug_msgs.msg import Float32Stamped +import sensor_msgs_py.point_cloud2 as pc2 +import sensor_msgs.msg as sensor_msgs +import numpy as np import tqdm -class FixQueue: + +class FixQueue(): def __init__(self): - self.queue_limit = 100 # Max number of items in the queue - self.data_queue = None # Queue of data - self.head = 0 # Index to append a new item (enqueue index) - self.tail = 0 # Index to start remove/process items (dequeue index) - self.current_size = 0 # Number of items in the queue + self.queue_limit = 100 # Max number of items in the queue + self.data_queue = None # Queue of data + self.head = 0 # Index to append a new item (enqueue index) + self.tail = 0 # Index to start remove/process items (dequeue index) + self.current_size = 0 # Number of items in the queue def enqueue(self, item): if self.is_full(): @@ -29,7 +44,7 @@ def enqueue(self, item): return False if self.is_empty() or self.data_queue.shape[1] != len(item): - self.data_queue = np.ndarray((self.queue_limit, len(item)), dtype=object) + self.data_queue = np.ndarray((self.queue_limit, len(item)), dtype = object) self.data_queue[self.tail, :] = item self.tail += 1 @@ -40,7 +55,7 @@ def enqueue(self, item): return True - def drop(self, limitless=False, callback_func=None, *args): + def drop(self, limitless = False, callback_func = None, *args): if limitless == True: end_id = self.tail else: @@ -65,17 +80,16 @@ def is_full(self) -> bool: if self.current_size == self.queue_limit: return True return False - + def is_empty(self) -> bool: if self.current_size == 0: return True return False - # Some utility functions that is shared by both the tp collector and tp checker # Convert a pose to a 4x4 transformation matrix def __pose_to_mat(pose: Pose) -> np.array: - t = pose.position # Translation + t = pose.position # Translation q = pose.orientation # Rotation qx = q.x @@ -83,22 +97,16 @@ def __pose_to_mat(pose: Pose) -> np.array: qz = q.z qw = q.w - return np.asarray( - [ - [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qw * qz), 2 * (qw * qy + qx * qz), t.x], - [2 * (qx * qy + qw * qz), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qw * qz), t.y], - [2 * (qx * qz - qw * qy), 2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy), t.z], - [0.0, 0.0, 0.0, 1.0], - ] - ) - + return np.asarray([[1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qw * qz), 2 * (qw * qy + qx * qz), t.x], + [2 * (qx * qy + qw * qz), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qw * qz), t.y], + [2 * (qx * qz - qw * qy), 2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy), t.z], + [0.0, 0.0, 0.0, 1.0]]) -# Point-wise transform +# Point-wise transform def transform_p(p, trans_mat_t: np.ndarray) -> np.ndarray: tp = np.asarray([p[0], p[1], p[2], 1.0]) return np.matmul(tp, trans_mat_t) - ##### Stamp search ##### def __stamp_search(stamp: int, ref_df: np.ndarray, search_size: int) -> int: left = 0 @@ -118,46 +126,47 @@ def __stamp_search(stamp: int, ref_df: np.ndarray, search_size: int) -> int: return res - def __get_message_count(rosbag_path: str): info = Info() metadata = info.read_metadata(rosbag_path, "sqlite3") - output = { - item.topic_metadata.name: item.message_count for item in metadata.topics_with_message_count - } + output = {item.topic_metadata.name : item.message_count for item in metadata.topics_with_message_count} output["total"] = metadata.message_count return output - -def __scan_callback( - scan_tuple, pose_df, pose_df_len, tp_df, tp_df_len, resolution, callback_func=None, *args -): +def __scan_callback(scan_tuple, + pose_df, + pose_df_len, + tp_df, + tp_df_len, + resolution, + callback_func =None, + *args): stamp, scan = scan_tuple - tmp_scan = pc2.read_points(scan, skip_nans=True) + tmp_scan = pc2.read_points(scan, skip_nans = True) - # Find the closest pose + # Find the closest pose pid = __stamp_search(stamp, pose_df, pose_df_len) if pid <= 0: return - + # Get the transposed transformation matrix closest_p_t = pose_df[pid, 1].T # Skip some 0 poses at the beginning of the rosbag if closest_p_t[3, 0] == 0 and closest_p_t[3, 1] == 0 and closest_p_t[3, 2] == 0: return - - # Find the closest tp + + # Find the closest tp tid = __stamp_search(stamp, tp_df, tp_df_len) if tid <= 0: return - + closest_tp = tp_df[tid, 1] - + # Transform the scan and find the segments that cover the transformed points # Record the segments that cover the scan segment_set = set() @@ -170,45 +179,38 @@ def __scan_callback( sy = int(tp[1] / resolution) * int(resolution) seg_idx = str(sx) + "_" + str(sy) - segment_set.add(seg_idx) + segment_set.add(seg_idx) # Update/examine the segments' avg TP callback_func(segment_set, closest_tp, *args) - ##### Read the input rosbag and update map's TP ##### -def collect_rosbag_tp(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str, *args): +def collect_rosbag_tp(bag_path: str, + pose_topic: str, + tp_topic: str, + scan_topic: str, + *args): reader = SequentialReader() - bag_storage_options = StorageOptions(uri=bag_path, storage_id="sqlite3") + bag_storage_options = StorageOptions(uri = bag_path, storage_id = "sqlite3") bag_converter_options = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" + input_serialization_format = "cdr", output_serialization_format = "cdr" ) reader.open(bag_storage_options, bag_converter_options) print("Reading rosbag...") msg_count_by_topic = __get_message_count(bag_path) - progress_bar = tqdm.tqdm(total=msg_count_by_topic["total"]) + progress_bar = tqdm.tqdm(total = msg_count_by_topic["total"]) if msg_count_by_topic[pose_topic] == 0: - print( - "Error: the input rosbag contains no message from the topic {0}. Exit!".format( - pose_topic - ) - ) + print("Error: the input rosbag contains no message from the topic {0}. Exit!".format(pose_topic)) if msg_count_by_topic[tp_topic] == 0: - print( - "Error: the input rosbag contains no message from the topic {0}. Exit!".format(tp_topic) - ) + print("Error: the input rosbag contains no message from the topic {0}. Exit!".format(tp_topic)) if msg_count_by_topic[scan_topic] == 0: - print( - "Error: the input rosbag contains no message from the topic {0}. Exit!".format( - scan_topic - ) - ) - - pose_df = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object) - tp_df = np.ndarray((msg_count_by_topic[tp_topic], 2), dtype=object) + print("Error: the input rosbag contains no message from the topic {0}. Exit!".format(scan_topic)) + + pose_df = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype = object) + tp_df = np.ndarray((msg_count_by_topic[tp_topic], 2), dtype = object) pose_df_idx = 0 tp_df_idx = 0 scan_df = FixQueue() @@ -219,7 +221,7 @@ def collect_rosbag_tp(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: progress_bar.update(1) (topic, data, stamp) = reader.read_next() - if skip_count <= 2000: + if (skip_count <= 2000): skip_count += 1 continue @@ -234,16 +236,16 @@ def collect_rosbag_tp(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: stamp = tp_msg.stamp.sec * 1e9 + tp_msg.stamp.nanosec tp_df[tp_df_idx, :] = [stamp, tp_msg.data] tp_df_idx += 1 - + elif topic == scan_topic: pc_msg = deserialize_message(data, sensor_msgs.PointCloud2) - stamp = pc_msg.header.stamp.sec * 1e9 + pc_msg.header.stamp.nanosec - + stamp = pc_msg.header.stamp.sec * 1e9 + pc_msg.header.stamp.nanosec + scan_df.enqueue([stamp, pc_msg]) if scan_df.is_full(): scan_df.drop(False, __scan_callback, pose_df, pose_df_idx, tp_df, tp_df_idx, *args) - + # Process the rest of the queue scan_df.drop(True, __scan_callback, pose_df, pose_df_idx, tp_df, tp_df_idx, *args) diff --git a/map/autoware_tp_collector/scripts/tp_visualizer.py b/map/autoware_tp_manager/scripts/tp_visualizer.py similarity index 72% rename from map/autoware_tp_collector/scripts/tp_visualizer.py rename to map/autoware_tp_manager/scripts/tp_visualizer.py index ac95c4c66..d5bf79ec2 100644 --- a/map/autoware_tp_collector/scripts/tp_visualizer.py +++ b/map/autoware_tp_manager/scripts/tp_visualizer.py @@ -14,56 +14,55 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os import argparse -import csv +from rosbag2_py import ( + SequentialReader, + StorageOptions, + ConverterOptions, + SequentialWriter, + BagMetadata, + TopicMetadata, + Info +) + +from rclpy.serialization import deserialize_message, serialize_message import os -import struct -import time - -from builtin_interfaces.msg import Time -from geometry_msgs.msg import PoseWithCovarianceStamped +import csv +import yaml +from scipy import spatial as sp import numpy as np -import open3d as o3d +from geometry_msgs.msg import PoseWithCovarianceStamped +from tier4_debug_msgs.msg import Float32Stamped +from builtin_interfaces.msg import Time import pandas as pd -import rclpy -from rclpy.node import Node -from rclpy.serialization import deserialize_message -from rclpy.serialization import serialize_message -from rosbag2_py import BagMetadata -from rosbag2_py import ConverterOptions -from rosbag2_py import Info -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions -from rosbag2_py import TopicMetadata -from scipy import spatial as sp +import tqdm +import open3d as o3d import sensor_msgs.msg as sensor_msgs -import sensor_msgs_py.point_cloud2 as pc2 import std_msgs.msg as std_msgs -from tier4_debug_msgs.msg import Float32Stamped -import tqdm -import yaml - +import sensor_msgs_py.point_cloud2 as pc2 +import rclpy +from rclpy.node import Node +import time +import struct class TPVisualizer(Node): def __init__(self): - super().__init__("tp_visualizer") + super().__init__('tp_visualizer') self.pcd_map_dir = None self.pcd_path = None self.yaml_path = None self.score_path = None # Color based on rounded TP - self.color = { - 0: [255, 255, 255], # White - 1: [255, 255, 51], # Yellow - 2: [255, 128, 0], # Orange - 3: [204, 0, 0], # Dark red - 4: [51, 153, 255], # Blue - 5: [51, 153, 255], - 6: [51, 153, 255], - 7: [51, 153, 255], - 8: [51, 153, 255], - } + self.color = {0 : [255, 255, 255], # White + 1 : [255, 255, 51], # Yellow + 2 : [255, 128, 0], # Orange + 3 : [204, 0, 0], # Dark red + 4 : [51, 153, 255], # Blue + 5 : [51, 153, 255], + 6 : [51, 153, 255], + 7 : [51, 153, 255], + 8 : [51, 153, 255]} ##### Read the YAML file to get the list of PCD segments and scores ##### def __get_pcd_segments_and_scores(self, pcd_map_dir: str): @@ -92,21 +91,15 @@ 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 tuple in self.segment_df.itertuples(): @@ -132,20 +125,19 @@ 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_visualizer", 10 - ) + pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, '/autoware_tp_visualizer', 10) while True: pcd_publisher.publish(pc2_msg) time.sleep(1) + def __set_color_based_on_score(self, score) -> int: r, g, b = self.color[int(score)] 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 @@ -155,7 +147,6 @@ def processing(self, pcd_map_dir: str): # Publish to rviz self.__show() - if __name__ == "__main__": rclpy.init() parser = argparse.ArgumentParser()