From c125f96f0ebb975459424e0a06bdbcc420f75c4f Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Thu, 9 Jan 2025 11:02:34 +0700 Subject: [PATCH 1/7] Adding tp collector to measure average TP of PCD segments Signed-off-by: Anh Nguyen --- map/autoware_tp_collector/CMakeLists.txt | 37 ++ map/autoware_tp_collector/LICENSE | 28 ++ map/autoware_tp_collector/README.md | 48 +++ .../config/pointcloud_merger.param.yaml | 7 + .../autoware/pointcloud_merger/pcd_merger.hpp | 108 ++++++ .../launch/pointcloud_merger.launch.py | 81 ++++ .../launch/pointcloud_merger.launch.xml | 9 + map/autoware_tp_collector/package.xml | 22 ++ .../schema/pointcloud_merger.schema.json | 48 +++ .../scripts/tp_collector.py | 366 ++++++++++++++++++ .../src/include/pointcloud_merger_node.hpp | 34 ++ map/autoware_tp_collector/src/pcd_merger.cpp | 221 +++++++++++ .../src/pointcloud_merger_node.cpp | 80 ++++ 13 files changed, 1089 insertions(+) create mode 100644 map/autoware_tp_collector/CMakeLists.txt create mode 100644 map/autoware_tp_collector/LICENSE create mode 100644 map/autoware_tp_collector/README.md create mode 100644 map/autoware_tp_collector/config/pointcloud_merger.param.yaml create mode 100644 map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp create mode 100644 map/autoware_tp_collector/launch/pointcloud_merger.launch.py create mode 100644 map/autoware_tp_collector/launch/pointcloud_merger.launch.xml create mode 100644 map/autoware_tp_collector/package.xml create mode 100644 map/autoware_tp_collector/schema/pointcloud_merger.schema.json create mode 100755 map/autoware_tp_collector/scripts/tp_collector.py create mode 100644 map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp create mode 100644 map/autoware_tp_collector/src/pcd_merger.cpp create mode 100644 map/autoware_tp_collector/src/pointcloud_merger_node.cpp diff --git a/map/autoware_tp_collector/CMakeLists.txt b/map/autoware_tp_collector/CMakeLists.txt new file mode 100644 index 00000000..4183518b --- /dev/null +++ b/map/autoware_tp_collector/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_pointcloud_merger) + +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) + +include_directories(include) + +ament_auto_add_library(${PROJECT_NAME} SHARED src/pointcloud_merger_node.cpp src/pcd_merger.cpp) +target_link_libraries(${PROJECT_NAME} yaml-cpp ${PCL_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} autoware_pointcloud_divider) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::pointcloud_merger::PointCloudMerger" + EXECUTABLE ${PROJECT_NAME}_node +) + +install(TARGETS ${PROJECT_NAME}_node + EXPORT ${PROJECT_NAME}_node + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include + ) + +ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/map/autoware_tp_collector/LICENSE b/map/autoware_tp_collector/LICENSE new file mode 100644 index 00000000..da934de9 --- /dev/null +++ b/map/autoware_tp_collector/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2023, MAP IV + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/map/autoware_tp_collector/README.md b/map/autoware_tp_collector/README.md new file mode 100644 index 00000000..2197dda9 --- /dev/null +++ b/map/autoware_tp_collector/README.md @@ -0,0 +1,48 @@ +# 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/config/pointcloud_merger.param.yaml b/map/autoware_tp_collector/config/pointcloud_merger.param.yaml new file mode 100644 index 00000000..e91d4690 --- /dev/null +++ b/map/autoware_tp_collector/config/pointcloud_merger.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + # Downsampling resolution + leaf_size: -0.1 + input_pcd_dir: $(var input_pcd_dir) # Path to the folder containing the input PCD Files + output_pcd: $(var output_pcd) # Path to the merged PCD File + point_type: "point_xyzi" # Type of points when processing PCD files diff --git a/map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp b/map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp new file mode 100644 index 00000000..a26abb37 --- /dev/null +++ b/map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp @@ -0,0 +1,108 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +// BSD 3-Clause License +// +// Copyright (c) 2023, MAP IV +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ +#define AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ + +#include + +#include +#include + +#define PCL_NO_PRECOMPILE +#include +#include + +#include +#include + +namespace autoware::pointcloud_merger +{ + +template +class PCDMerger +{ + typedef pcl::PointCloud PclCloudType; + typedef typename PclCloudType::Ptr PclCloudPtr; + +public: + explicit PCDMerger(const rclcpp::Logger & logger) : logger_(logger) {} + + void setInput(const std::string & input) { input_dir_ = input; } + + void setOutput(const std::string & output) { output_pcd_ = output; } + + void setConfig(const std::string & config_file) + { + config_file_ = config_file; + + paramInitialize(); + } + + void setLeafSize(double leaf_size) { leaf_size_ = leaf_size; } + + void run(); + void run(const std::vector & pcd_names); + +private: + std::string output_pcd_, config_file_, input_dir_; + + // Params from yaml + double leaf_size_ = 0.1; + + // Maximum number of points per PCD block + const size_t max_block_size_ = 500000; + + std::string tmp_dir_; + autoware::pointcloud_divider::CustomPCDWriter writer_; + rclcpp::Logger logger_; + + std::vector discoverPCDs(const std::string & input); + void paramInitialize(); + void mergeWithoutDownsample(const std::vector & input_pcds); + void mergeWithDownsample(const std::vector & input_pcds); +}; + +} // namespace autoware::pointcloud_merger + +#endif // AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ diff --git a/map/autoware_tp_collector/launch/pointcloud_merger.launch.py b/map/autoware_tp_collector/launch/pointcloud_merger.launch.py new file mode 100644 index 00000000..f3a1607b --- /dev/null +++ b/map/autoware_tp_collector/launch/pointcloud_merger.launch.py @@ -0,0 +1,81 @@ +# 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. + +import os + +from ament_index_python import get_package_share_path +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + arg = DeclareLaunchArgument(name, default_value=default_value, description=description) + launch_arguments.append(arg) + + add_launch_arg("input_pcd_dir", "", "Path to the folder containing the input PCD files") + add_launch_arg("output_pcd", "", "Path to the merged PCD file") + add_launch_arg( + "config_file", + [FindPackageShare("autoware_pointcloud_merger"), "/config/default.param.yaml"], + "Path to the parameter file of the package autoware_pointcloud_merger", + ) + + # Parameter of pointcloud divider + config_file = os.path.join( + get_package_share_path("autoware_pointcloud_merger"), "config", "default.param.yaml" + ) + with open(config_file, "r") as f: + merger_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + merger_node = Node( + package="autoware_pointcloud_merger", + executable="autoware_pointcloud_merger_node", + parameters=[ + merger_param, + { + "input_pcd_dir": LaunchConfiguration("input_pcd_dir"), + "output_pcd": LaunchConfiguration("output_pcd"), + "config_file": LaunchConfiguration("config_file"), + }, + ], + output="screen", + ) + merger_shutdown = get_shutdown_handler(merger_node) + + return launch.LaunchDescription( + launch_arguments + + [ + merger_node, + merger_shutdown, + ] + ) + + +def get_shutdown_handler(node): + return launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=node, + on_exit=[ + launch.actions.LogInfo(msg="shutdown launch"), + launch.actions.EmitEvent(event=launch.events.Shutdown()), + ], + ) + ) diff --git a/map/autoware_tp_collector/launch/pointcloud_merger.launch.xml b/map/autoware_tp_collector/launch/pointcloud_merger.launch.xml new file mode 100644 index 00000000..082ffb2c --- /dev/null +++ b/map/autoware_tp_collector/launch/pointcloud_merger.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/map/autoware_tp_collector/package.xml b/map/autoware_tp_collector/package.xml new file mode 100644 index 00000000..08ac368b --- /dev/null +++ b/map/autoware_tp_collector/package.xml @@ -0,0 +1,22 @@ + + + + autoware_pointcloud_merger + 0.1.0 + A package for dividing PCD files to non-overlapped segments + Anh Nguyen + Apache License 2.0 + + Anh Nguyen + + ament_cmake_auto + autoware_cmake + + autoware_pointcloud_divider + libpcl-all-dev + yaml-cpp + + + ament_cmake + + diff --git a/map/autoware_tp_collector/schema/pointcloud_merger.schema.json b/map/autoware_tp_collector/schema/pointcloud_merger.schema.json new file mode 100644 index 00000000..21311d69 --- /dev/null +++ b/map/autoware_tp_collector/schema/pointcloud_merger.schema.json @@ -0,0 +1,48 @@ +{ + "$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/scripts/tp_collector.py b/map/autoware_tp_collector/scripts/tp_collector.py new file mode 100755 index 00000000..9c194035 --- /dev/null +++ b/map/autoware_tp_collector/scripts/tp_collector.py @@ -0,0 +1,366 @@ +#!/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. + +import os +import argparse +from rosbag2_py import ( + SequentialReader, + StorageOptions, + ConverterOptions, + SequentialWriter, + BagMetadata, + TopicMetadata, + Info +) + +from rclpy.serialization import deserialize_message, serialize_message +from subprocess import call, run, Popen +import os +import time +import csv +import yaml +import shutil + +##### Get the list of PCD segments ##### +def get_pcd_segments(pcd_map_dir: str): + if not os.path.exists(pcd_map_dir): + print("Error: %s does not exist!"%(pcd_map_dir)) + exit() + + pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd/") + + if not os.path.exists(pcd_path): + print("Error: %s does not exist!"%(pcd_path)) + exit() + + # Get list of PCD segments in the folder pointcloud_map.pcd + pcd_segment_list = [] + + for filename in os.listdir(pcd_path): + if filename.endswith(".pcd") or filename.endswith(".PCD"): + segment_path = os.path.join(pcd_path, filename) + pcd_segment_list.append(segment_path) + + + + + + +##### Setup the working directory +def setup(work_dir: str): + if os.path.exists(work_dir): + if os.path.isdir(work_dir): + print("%s already exists. Do you want to overwrite its content? [y/n]"%(work_dir)) + else: + print("%s is not a directory. Do you want to delete it and create a new directory? [y/n]"%(work_dir)) + if input() != "y": + return + shutil.rmtree(work_dir) + os.mkdir(work_dir) + +##### Search for the topic list from the metadata +def topic_search(topic_list, metadata): + """ + This function iterates on each topic in the topic list, + and search for each topic from the metadata string. + :param topic_list: list of topic string + :param metadata: dict of {topic_name : number of messages} + """ + + output_list = [] + + for topic in topic_list: + value = metadata.get(topic, "unknown") + if value != "unknown": + output_list.append(topic) + + return output_list + +###### Step 0: Check if some topics exist in the rosbag +# Print the metadata of the rosbag and look for the topics in the metadata +def topic_setup(topic_dict: dict, in_bag_dir: str): + # Setup lidar topic + lidar_topics_candidate = [ + # "/sensing/lidar/concatenated/pointcloud", + "/sensing/lidar/left_upper/pandar_packets", + # "/sensing/lidar/right_upper/pandar_packets", + # "/sensing/lidar/rear_upper/pandar_packets", + # "/sensing/lidar/right_lower/pandar_packets", + # "/sensing/lidar/left_lower/pandar_packets", + # "/sensing/lidar/front_lower/pandar_packets", + # "/sensing/lidar/front_upper/pandar_packets", + "/sensing/lidar/left_upper/pointcloud_raw_ex" + ] + # Setup GNSS topic + gnss_topic_candidate = ["/sensing/gnss/septentrio/nav_sat_fix", + "/sensing/gnss/ublox/nav_sat_fix", + "/sensing/gnss/nav_sat_fix"] + # Setup imu topic + imu_topic_candidate = ["/sensing/imu/imu_data", + "/sensing/imu/tamagawa/imu_raw"] + # Setup tf_static topic + tf_static_topic_candidate = ["/tf_static"] + # Setup velocity_status topic + velocity_status_topic_candidate = ["/vehicle/status/velocity_status"] + + # Setup a dictionary for rosbag metadata + bag_path = "" + for (dirpath, dirnames, filenames) in os.walk(in_bag_dir): + for fname in filenames: + if fname.endswith(".db3"): + bag_path = dirpath + "/" + fname + break + + info = Info() + metadata = info.read_metadata(bag_path, "sqlite3") + bag_topic_dict = {} + + for topic_info in metadata.topics_with_message_count: + bag_topic_dict[topic_info.topic_metadata.name] = topic_info.topic_metadata.type + + topic_dict["lidar"] = topic_search(lidar_topics_candidate, bag_topic_dict) + topic_dict["gnss"] = topic_search(gnss_topic_candidate, bag_topic_dict) + topic_dict["imu"] = topic_search(imu_topic_candidate, bag_topic_dict) + topic_dict["tf_static"] = topic_search(tf_static_topic_candidate, bag_topic_dict) + topic_dict["velocity_status"] = topic_search(velocity_status_topic_candidate, bag_topic_dict) + + print(topic_dict) + +# Filter a rosbag to keep specified topcis only +def rosbag_filter(in_bag: str, out_bag: str, topic_mark: dict): + reader = SequentialReader() + bag_storage_options = StorageOptions(uri = in_bag, storage_id = "sqlite3") + bag_converter_options = ConverterOptions(input_serialization_format = "cdr", output_serialization_format = "cdr") + reader.open(bag_storage_options, bag_converter_options) + + writer = SequentialWriter() + bag_storage_options = StorageOptions(uri = out_bag, storage_id = "sqlite3") + bag_converter_options = ConverterOptions(input_serialization_format = "cdr", output_serialization_format = "cdr") + writer.open(bag_storage_options, bag_converter_options) + + all_topic_metadata = reader.get_all_topics_and_types() + + for topic_metadata in all_topic_metadata: + if topic_metadata.name in topic_mark: + writer.create_topic( + TopicMetadata( + name = topic_metadata.name, + type = topic_metadata.type, + serialization_format = "cdr" + ) + ) + + while reader.has_next(): + (topic, data, stamp) = reader.read_next() + + if topic in topic_mark: + writer.write(topic, data, stamp) + +# Step 1: Pre-processing the input rosbags +# Remove unused topics and merge small rosbags to a big one +def preprocessing(topic_dict: dict, in_bag_dir: str, work_dir: str): + """ + Filtering rosbag to extract necessary topics only, also + generate a metadata.yaml for the filtered rosbag. + :param topic_dict: dict, a dictionary of required topics from the input rosbag + :param in_bag_dir: str, path to the input rosbag + :param work_dir: str, path to the working directory, which contains all temporal and output data + """ + # If the merged rosbag already exist, quit + merged_bag = os.path.normpath(work_dir + "/merged.db3") + if os.path.exists(merged_bag): + print("Merged rosbag already exists at %s. Do you want to re-create? [y/n]"%(merged_bag)) + if input() != "y": + return + + # Scan the input bag directory and get all files having the .db3 extension + bag_list = [] + filtered_bag_list = [] + for (dirpath, dirnames, filenames) in os.walk(in_bag_dir): + for fname in filenames: + if fname.endswith(".db3"): + bag_list.append(dirpath + fname) + + bag_list.sort() + + topic_set = set() + + for (topic_tag, topic_names) in topic_dict.items(): + for name in topic_names: + print("Adding topic %s"%(name)) + topic_set.add(name) + + # Filter every input rosbag + for in_bag in bag_list: + filtered_in_bag = work_dir + "/" + os.path.splitext(os.path.basename(in_bag))[0] + "_filtered.db3" + rosbag_filter(in_bag, filtered_in_bag, topic_set) + filtered_bag_list.append(filtered_in_bag) + + # Merge all filtered rosbag to a single one + merge_cmd = "ros2 bag merge -o " + merged_bag + for filtered_in_bag in filtered_bag_list: + merge_cmd += " " + filtered_in_bag + + call(merge_cmd, shell = True) + + # Delete all filtered rosbags, only keep the merged rosbag + for filtered_in_bag in filtered_bag_list: + call("rm -fr " + filtered_in_bag, shell = True) + +# Step 2: compare the rosbag with the PCD map and detect changed areas +def map_change_checking(topic_dict: dict, work_dir: str, map_path: str, m4e_mount_path: str): + """ + Compare the filtered rosbag with the PCD map, and detect map areas that changed. + Filter the filtered rosbag to extract scans that cover the changed areas only, + and remove invalid GNSS messages from the filtered rosbag. Convert the remaining + of the filtered rosbag to the ROS1 format. + """ + # Generate rosbag for evaluation + eval_bag = os.path.normpath(work_dir + "/eval_bag") + vehicle_model = "j6_gen1" + sensor_model = "aip_x2" + + # Launch pilot-auto.x2's logging simulator + # Popen(["ros2", "launch", "autoware_launch", "logging_simulator.launch.xml", \ + # "map_path:=" + map_path, "vehicle_model:=j6_gen1", "sensor_model:=aip_x2"]) + # # ,\ + # # "perception:=false", "planning:=false", "control:=false"]) + # Wait for the logging simulator finishes setting up + # time.sleep(360) + # record_proc = Popen(["ros2", "bag", "record", "-o", eval_bag, "/localization/pose_twist_fusion_filter/biased_pose_with_covariance", \ + # "/localization/util/downsample/pointcloud"]) + # ros2 bag record -o eval_bag /localization/pose_twist_fusion_filter/biased_pose_with_covariance /localization/util/downsample/pointcloud + + # Play the rosbag and wait for it to finish + # call("ros2 bag play " + work_dir + "/merged.db3 -r 2.0 -s sqlite3 --clock 100", shell = True) + + # Quit + # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -2 $(pgrep -P \", $1, \") > /dev/null 2>&1\" }' | sh") + # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -2 \", $1, \" > /dev/null 2>&1\" }' | sh") + # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -9 $(pgrep -P \", $1, \") > /dev/null 2>&1\" }' | sh") + # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -9 \", $1, \" > /dev/null 2>&1\" }' | sh") + + # # Check if the eval rosbag exists + # if not(os.path.exists(eval_bag)): + # print("Evaluation rosbag does not exist at %s. Exit..."%(eval_bag)) + # exit() + # else: + # print("Evaluation rosbag exists at %s."%(eval_bag)) + + # # Run map_assessment_tools + # mast_output = os.path.normpath(work_dir + "/mast_result/") + + # mast_cmd = "ros2 launch map_assessment_tools map_evaluation.launch.py rosbag_file_name:=" + \ + # eval_bag + " map_path:=" + map_path + " save_dir:=" + mast_output + \ + # " sensor_topic_name:=/localization/util/downsample/pointcloud roc:=true" + + # if not(os.path.exists(mast_output)): + # os.mkdir(mast_output) + # call(mast_cmd, shell = True) + # else: + # print("map_assessment_tools output already exists. Do you want to re-run? [y/n]") + # if input() == "y": + # call(mast_cmd, shell = True) + + # Filter the merged bag to keep the scans that cover the changed area + # Also remove invalid GNSS messages + final_bag = os.path.normpath(work_dir + "/final_bag.db3") + filter_cmd = "ros2 run map_assessment_tools rosbag_interval_filter.py " + \ + work_dir + "/merged.db3 " + final_bag + \ + " --gnss_topic " + ",".join(topic_dict["gnss"]) + \ + " --sensor_topic " + ",".join(topic_dict["lidar"]) + # " --save_dir " + mast_output + \ + # " --gnss_topic " + topic_dict["gnss"] + \ + # " --sensor_topic " + topic_dict["lidar"] + + if os.path.exists(final_bag): + print("The filtered final ROS2 rosbag already exists. Do you want to re-filter? [y/n]") + if input() == "y": + shutil.rmtree(final_bag) + call(filter_cmd, shell = True) + else: + call(filter_cmd, shell = True) + + # Convert the final rosbag from ROS2 format to ROS1 format + print("Convert to ROS1 format...") + call("/home/anh/Work/rosbags/venv/bin/rosbags-convert-2to1 " + final_bag, shell = True) + # Move the ROS1 bag to map4-cli output path + m4e_path = os.path.normpath(m4e_mount_path + "/" + os.path.basename(work_dir) + "/") + if os.path.exists(m4e_path): + print("map4-cli mount path already exists at %s. Do you want to re-create? [y/n]"%(m4e_path)) + if input() == "y": + shutil.rmtree(m4e_path) + os.mkdir(m4e_path) + call("mv " + final_bag + ".bag " + m4e_path + "/input_rosbag.bag", shell = True) + print("A ROS1 rosbag was copied to %s."%(m4e_path + "/input_rosbag.bag")) + else: + os.mkdir(m4e_path) + call("mv " + final_bag + ".bag " + m4e_path + "/input_rosbag.bag", shell = True) + print("A ROS1 rosbag was copied to %s."%(m4e_path + "/input_rosbag.bag")) + + # Merge pcd files and put them to m4e_path/map + m4e_map_path = os.path.normpath(m4e_path + "/map") + if os.path.exists(m4e_map_path): + print("The map folder for PCD update already exists at %s. Do you want to re-create? [y/n]"%(m4e_map_path)) + if input() == "y": + shutil.rmtree(m4e_map_path) + os.mkdir(m4e_map_path) + else: + os.mkdir(m4e_map_path) + + merge_cmd = "ros2 launch autoware_pointcloud_merger pointcloud_merger.launch.xml " + \ + "input_pcd_dir:=" + map_path + "/pointcloud_map.pcd/" + \ + " output_pcd:=" + m4e_map_path + "/merged.pcd" + + call(merge_cmd, shell = True) + print("PCD Update preparation is done.") + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("map_path", help="The path to the folder containing PCD files of the old map") + parser.add_argument("bag_path", help="The path to the input rosbag") + parser.add_argument("output_path", help="The path to the folder containing the updated PCD files") + parser.add_argument("--type", help="Type of launcher pilot-auto.x2/pilot-auto.xx1", default="pilot-auto.x2", required=False) + + args = parser.parse_args() + + # Practice with string % a bit + print("Input PCD map at %s" % (args.map_path)) + print("Input rosbag at %s" % (args.bag_path)) + print("Output PCD map at %s" % (args.output_path)) + + # Step 0: Setup required topics + # To evaluate and update map, the filtered rosbag must contain + # IMU, GNSS, Lidar, TF Static, and velocity status + topic_dict = { + "lidar": "unknown", + "gnss": "unknown", + "imu": "unknown", + "tf_static": "unknown", + "velocity_status": "unknown", + } + # Cleanup the working directory + work_dir = os.path.normpath(args.output_path) + setup(work_dir) + # Setup topics for filtering and merge + topic_setup(topic_dict, args.bag_path) + # Step 1: Pre-process rosbag + preprocessing(topic_dict, args.bag_path, work_dir) + # Step 2: Check map changes + m4e_mount_path = os.path.normpath("/media/anh/AnhNguyen/map4_engine_output/") + map_change_checking(topic_dict, work_dir, args.map_path, m4e_mount_path) + # Step 3: Update PCD map + # Step 4: Post validation diff --git a/map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp b/map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp new file mode 100644 index 00000000..2cecbb45 --- /dev/null +++ b/map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp @@ -0,0 +1,34 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#ifndef POINTCLOUD_MERGER_NODE_HPP_ +#define POINTCLOUD_MERGER_NODE_HPP_ + +#include + +#define PCL_NO_RECOMPILE +#include + +namespace autoware::pointcloud_merger +{ + +class PointCloudMerger : public rclcpp::Node +{ +public: + explicit PointCloudMerger(const rclcpp::NodeOptions & node_options); +}; + +} // namespace autoware::pointcloud_merger + +#endif // POINTCLOUD_MERGER_NODE_HPP_ diff --git a/map/autoware_tp_collector/src/pcd_merger.cpp b/map/autoware_tp_collector/src/pcd_merger.cpp new file mode 100644 index 00000000..d4cf5869 --- /dev/null +++ b/map/autoware_tp_collector/src/pcd_merger.cpp @@ -0,0 +1,221 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +// BSD 3-Clause License +// +// Copyright (c) 2023, MAP IV +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "include/pointcloud_merger_node.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +namespace fs = std::filesystem; + +namespace autoware::pointcloud_merger +{ + +template +std::vector PCDMerger::discoverPCDs(const std::string & input) +{ + fs::path input_path(input); + + if (!fs::is_directory(input_path)) { + RCLCPP_ERROR(logger_, "Error: Invalid input directory %s", input.c_str()); + rclcpp::shutdown(); + } + + std::vector pcd_list; + + for (auto & entry : fs::directory_iterator(input_path)) { + if (fs::is_regular_file(entry.symlink_status())) { + auto file_name = entry.path().string(); + auto extension = entry.path().extension().string(); + + if (extension == ".pcd" || extension == ".PCD") { + pcd_list.push_back(file_name); + } + } + } + + RCLCPP_INFO(logger_, "Found %lu PCD files", pcd_list.size()); + + return pcd_list; +} + +template +void PCDMerger::run() +{ + auto pcd_list = discoverPCDs(input_dir_); + + run(pcd_list); +} + +template +void PCDMerger::run(const std::vector & pcd_names) +{ + // Just in case the downsampling option is on + if (leaf_size_ > 0) { + tmp_dir_ = "./pointcloud_merger_tmp/"; + + if (fs::exists(tmp_dir_)) { + fs::remove_all(tmp_dir_); + } + + autoware::pointcloud_divider::util::make_dir(tmp_dir_); + } + + if (fs::exists(output_pcd_)) { + fs::remove_all(output_pcd_); + } + + if (leaf_size_ > 0) { + mergeWithDownsample(pcd_names); + autoware::pointcloud_divider::util::remove(tmp_dir_); + } else { + mergeWithoutDownsample(pcd_names); + } +} + +template +void PCDMerger::mergeWithDownsample(const std::vector & input_pcds) +{ + RCLCPP_INFO(logger_, "Downsampling by PointCloudDivider"); + // Divide the input clouds to smaller segments + autoware::pointcloud_divider::PCDDivider pcd_divider(logger_); + + pcd_divider.setOutputDir(tmp_dir_); + pcd_divider.setGridSize(leaf_size_ * 100, leaf_size_ * 100); + pcd_divider.setPrefix("tmp_segment"); + pcd_divider.setLeafSize(leaf_size_); + pcd_divider.setDebugMode(false); + pcd_divider.run(input_pcds); + + // Now go get path of the segmentation pcd + std::vector seg_names; + fs::path tmp_path(tmp_dir_); + + for (auto & entry : fs::recursive_directory_iterator(tmp_path)) { + if (fs::is_regular_file(entry.symlink_status())) { + auto fname = entry.path().string(); + auto ext = fname.substr(fname.size() - 4); + + if (ext == ".pcd") { + seg_names.push_back(fname); + } + } + } + + // Now merge the downsampled segments + mergeWithoutDownsample(seg_names); +} + +template +void PCDMerger::mergeWithoutDownsample(const std::vector & input_pcds) +{ + if (input_pcds.size() == 0) { + RCLCPP_INFO(logger_, "No input PCDs. Return!"); + + return; + } + + // Check the number of points of the merger + size_t total_point_num = 0; + autoware::pointcloud_divider::CustomPCDReader reader; + + for (const auto & pcd_name : input_pcds) { + reader.setInput(pcd_name); + + total_point_num += reader.point_num(); + } + + writer_.setOutput(output_pcd_); + writer_.writeMetadata(total_point_num, true); + + size_t file_counter = 0; + + for (const auto & pcd_name : input_pcds) { + if (!rclcpp::ok()) { + return; + } + + RCLCPP_INFO( + logger_, "Processing file [%lu/%lu] %s", file_counter, input_pcds.size(), pcd_name.c_str()); + ++file_counter; + + reader.setInput(pcd_name); + + do { + PclCloudType new_cloud; + + reader.readABlock(new_cloud); + writer_.write(new_cloud); + } while (reader.good() && rclcpp::ok()); + } +} + +template +void PCDMerger::paramInitialize() +{ + try { + YAML::Node conf = YAML::LoadFile(config_file_); + auto params = conf["/**"]["ros__parameters"]; + + leaf_size_ = params["leaf_size"].as(); + } catch (YAML::Exception & e) { + RCLCPP_ERROR(logger_, "YAML Error: %s", e.what()); + rclcpp::shutdown(); + exit(EXIT_FAILURE); + } +} + +template class PCDMerger; +template class PCDMerger; +// template class PCDMerger; +// template class PCDMerger; +// template class PCDMerger; + +} // namespace autoware::pointcloud_merger diff --git a/map/autoware_tp_collector/src/pointcloud_merger_node.cpp b/map/autoware_tp_collector/src/pointcloud_merger_node.cpp new file mode 100644 index 00000000..4a720703 --- /dev/null +++ b/map/autoware_tp_collector/src/pointcloud_merger_node.cpp @@ -0,0 +1,80 @@ +// Copyright 2024 Autoware Foundation +// +// 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. + +#include "include/pointcloud_merger_node.hpp" + +#include + +#include + +#include + +namespace autoware::pointcloud_merger +{ + +PointCloudMerger::PointCloudMerger(const rclcpp::NodeOptions & node_options) +: Node("pointcloud_merger", node_options) +{ + // Load command parameters + float leaf_size = declare_parameter("leaf_size"); + std::string input_pcd_dir = declare_parameter("input_pcd_dir"); + std::string output_pcd = declare_parameter("output_pcd"); + std::string point_type = declare_parameter("point_type"); + + // Enter a new line and clear it + // This is to get rid of the prefix of RCLCPP_INFO + std::string line_breaker(102, ' '); + + line_breaker[0] = '\n'; + line_breaker[1] = line_breaker[101] = '\r'; + + std::ostringstream param_display; + + param_display << line_breaker << line_breaker << "########## Input Parameters ##########" + << line_breaker; + + param_display << "\tleaf_size: " << leaf_size << line_breaker; + param_display << "\tinput_pcd_dir: " << input_pcd_dir << line_breaker; + param_display << "\toutput_pcd: " << output_pcd << line_breaker; + param_display << "\tpoint_type: " << point_type << line_breaker; + param_display << "######################################" << line_breaker; + + RCLCPP_INFO(get_logger(), "%s", param_display.str().c_str()); + + if (point_type == "point_xyz") { + autoware::pointcloud_merger::PCDMerger pcd_merger_exe(get_logger()); + + pcd_merger_exe.setLeafSize(leaf_size); + pcd_merger_exe.setInput(input_pcd_dir); + pcd_merger_exe.setOutput(output_pcd); + + pcd_merger_exe.run(); + } else if (point_type == "point_xyzi") { + autoware::pointcloud_merger::PCDMerger pcd_merger_exe(get_logger()); + + pcd_merger_exe.setLeafSize(leaf_size); + pcd_merger_exe.setInput(input_pcd_dir); + pcd_merger_exe.setOutput(output_pcd); + + pcd_merger_exe.run(); + } + + rclcpp::shutdown(); +} + +} // namespace autoware::pointcloud_merger + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_merger::PointCloudMerger) From 4943fe3fe6fb68f7dbaebba89ccf8b5795fc8dd0 Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Wed, 15 Jan 2025 11:09:35 +0700 Subject: [PATCH 2/7] Building Signed-off-by: Anh Nguyen --- map/autoware_tp_collector/CMakeLists.txt | 23 +- .../config/pointcloud_merger.param.yaml | 7 - .../autoware/pointcloud_merger/pcd_merger.hpp | 108 ----- .../launch/pointcloud_merger.launch.py | 81 ---- .../launch/pointcloud_merger.launch.xml | 9 - map/autoware_tp_collector/package.xml | 5 +- .../scripts/tp_checker.py | 201 +++++++++ .../scripts/tp_collector.py | 421 ++++++------------ .../src/include/pointcloud_merger_node.hpp | 34 -- map/autoware_tp_collector/src/pcd_merger.cpp | 221 --------- .../src/pointcloud_merger_node.cpp | 80 ---- 11 files changed, 333 insertions(+), 857 deletions(-) delete mode 100644 map/autoware_tp_collector/config/pointcloud_merger.param.yaml delete mode 100644 map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp delete mode 100644 map/autoware_tp_collector/launch/pointcloud_merger.launch.py delete mode 100644 map/autoware_tp_collector/launch/pointcloud_merger.launch.xml create mode 100755 map/autoware_tp_collector/scripts/tp_checker.py delete mode 100644 map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp delete mode 100644 map/autoware_tp_collector/src/pcd_merger.cpp delete mode 100644 map/autoware_tp_collector/src/pointcloud_merger_node.cpp diff --git a/map/autoware_tp_collector/CMakeLists.txt b/map/autoware_tp_collector/CMakeLists.txt index 4183518b..773e0f9b 100644 --- a/map/autoware_tp_collector/CMakeLists.txt +++ b/map/autoware_tp_collector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(autoware_pointcloud_merger) +project(autoware_tp_manager) find_package(autoware_cmake REQUIRED) autoware_package() @@ -15,23 +15,10 @@ endif () find_package(yaml-cpp REQUIRED) find_package(PCL REQUIRED) -include_directories(include) - -ament_auto_add_library(${PROJECT_NAME} SHARED src/pointcloud_merger_node.cpp src/pcd_merger.cpp) -target_link_libraries(${PROJECT_NAME} yaml-cpp ${PCL_LIBRARIES}) -ament_target_dependencies(${PROJECT_NAME} autoware_pointcloud_divider) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "autoware::pointcloud_merger::PointCloudMerger" - EXECUTABLE ${PROJECT_NAME}_node +install(PROGRAMS + scripts/tp_collector.py + scripts/tp_checker.py + DESTINATION lib/${PROJECT_NAME} ) -install(TARGETS ${PROJECT_NAME}_node - EXPORT ${PROJECT_NAME}_node - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - INCLUDES DESTINATION include - ) - ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/map/autoware_tp_collector/config/pointcloud_merger.param.yaml b/map/autoware_tp_collector/config/pointcloud_merger.param.yaml deleted file mode 100644 index e91d4690..00000000 --- a/map/autoware_tp_collector/config/pointcloud_merger.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - # Downsampling resolution - leaf_size: -0.1 - input_pcd_dir: $(var input_pcd_dir) # Path to the folder containing the input PCD Files - output_pcd: $(var output_pcd) # Path to the merged PCD File - point_type: "point_xyzi" # Type of points when processing PCD files diff --git a/map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp b/map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp deleted file mode 100644 index a26abb37..00000000 --- a/map/autoware_tp_collector/include/autoware/pointcloud_merger/pcd_merger.hpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// 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. - -// BSD 3-Clause License -// -// Copyright (c) 2023, MAP IV -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#ifndef AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ -#define AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ - -#include - -#include -#include - -#define PCL_NO_PRECOMPILE -#include -#include - -#include -#include - -namespace autoware::pointcloud_merger -{ - -template -class PCDMerger -{ - typedef pcl::PointCloud PclCloudType; - typedef typename PclCloudType::Ptr PclCloudPtr; - -public: - explicit PCDMerger(const rclcpp::Logger & logger) : logger_(logger) {} - - void setInput(const std::string & input) { input_dir_ = input; } - - void setOutput(const std::string & output) { output_pcd_ = output; } - - void setConfig(const std::string & config_file) - { - config_file_ = config_file; - - paramInitialize(); - } - - void setLeafSize(double leaf_size) { leaf_size_ = leaf_size; } - - void run(); - void run(const std::vector & pcd_names); - -private: - std::string output_pcd_, config_file_, input_dir_; - - // Params from yaml - double leaf_size_ = 0.1; - - // Maximum number of points per PCD block - const size_t max_block_size_ = 500000; - - std::string tmp_dir_; - autoware::pointcloud_divider::CustomPCDWriter writer_; - rclcpp::Logger logger_; - - std::vector discoverPCDs(const std::string & input); - void paramInitialize(); - void mergeWithoutDownsample(const std::vector & input_pcds); - void mergeWithDownsample(const std::vector & input_pcds); -}; - -} // namespace autoware::pointcloud_merger - -#endif // AUTOWARE__POINTCLOUD_MERGER__PCD_MERGER_HPP_ diff --git a/map/autoware_tp_collector/launch/pointcloud_merger.launch.py b/map/autoware_tp_collector/launch/pointcloud_merger.launch.py deleted file mode 100644 index f3a1607b..00000000 --- a/map/autoware_tp_collector/launch/pointcloud_merger.launch.py +++ /dev/null @@ -1,81 +0,0 @@ -# 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. - -import os - -from ament_index_python import get_package_share_path -import launch -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -import yaml - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - arg = DeclareLaunchArgument(name, default_value=default_value, description=description) - launch_arguments.append(arg) - - add_launch_arg("input_pcd_dir", "", "Path to the folder containing the input PCD files") - add_launch_arg("output_pcd", "", "Path to the merged PCD file") - add_launch_arg( - "config_file", - [FindPackageShare("autoware_pointcloud_merger"), "/config/default.param.yaml"], - "Path to the parameter file of the package autoware_pointcloud_merger", - ) - - # Parameter of pointcloud divider - config_file = os.path.join( - get_package_share_path("autoware_pointcloud_merger"), "config", "default.param.yaml" - ) - with open(config_file, "r") as f: - merger_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - merger_node = Node( - package="autoware_pointcloud_merger", - executable="autoware_pointcloud_merger_node", - parameters=[ - merger_param, - { - "input_pcd_dir": LaunchConfiguration("input_pcd_dir"), - "output_pcd": LaunchConfiguration("output_pcd"), - "config_file": LaunchConfiguration("config_file"), - }, - ], - output="screen", - ) - merger_shutdown = get_shutdown_handler(merger_node) - - return launch.LaunchDescription( - launch_arguments - + [ - merger_node, - merger_shutdown, - ] - ) - - -def get_shutdown_handler(node): - return launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=node, - on_exit=[ - launch.actions.LogInfo(msg="shutdown launch"), - launch.actions.EmitEvent(event=launch.events.Shutdown()), - ], - ) - ) diff --git a/map/autoware_tp_collector/launch/pointcloud_merger.launch.xml b/map/autoware_tp_collector/launch/pointcloud_merger.launch.xml deleted file mode 100644 index 082ffb2c..00000000 --- a/map/autoware_tp_collector/launch/pointcloud_merger.launch.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/map/autoware_tp_collector/package.xml b/map/autoware_tp_collector/package.xml index 08ac368b..de3da52f 100644 --- a/map/autoware_tp_collector/package.xml +++ b/map/autoware_tp_collector/package.xml @@ -1,9 +1,9 @@ - autoware_pointcloud_merger + autoware_tp_manager 0.1.0 - A package for dividing PCD files to non-overlapped segments + A package for checking TP scores of NDT matching Anh Nguyen Apache License 2.0 @@ -12,7 +12,6 @@ ament_cmake_auto autoware_cmake - autoware_pointcloud_divider libpcl-all-dev yaml-cpp diff --git a/map/autoware_tp_collector/scripts/tp_checker.py b/map/autoware_tp_collector/scripts/tp_checker.py new file mode 100755 index 00000000..67953e4c --- /dev/null +++ b/map/autoware_tp_collector/scripts/tp_checker.py @@ -0,0 +1,201 @@ +#!/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. + +import os +import argparse +from rosbag2_py import ( + SequentialReader, + StorageOptions, + ConverterOptions, + SequentialWriter, + BagMetadata, + TopicMetadata, + 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 +from geometry_msgs.msg import PoseWithCovarianceStamped +from tier4_debug_msgs.msg import Float32Stamped +from builtin_interfaces.msg import Time +import pandas as pd + +##### Convert names of pcd segments to 2d numpy array ##### +def name_to_coordinate(seg_name: str): + # Remove the extension + name_only = os.path.splitext(seg_name)[0] + str_coors = name_only.split("_") + + # Return the coordinates as a 1x2 ndarray + return np.array(str_coors, dtype = float).reshape(1, 2) + +def get_pcd_segments_and_scores(pcd_map_dir: str) -> pd.DataFrame: + # Check if the required folders/files exist + if not os.path.exists(pcd_map_dir): + print("Error: {0} does not exist!".format(pcd_map_dir)) + exit() + + pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd") + + if not os.path.exists(pcd_path): + print("Error: {0} does not exist!".format(pcd_path)) + exit() + + metadata_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") + + if not os.path.exists(metadata_path): + print("Error: {0} does not exist!".format(metadata_path)) + exit() + + score_path = os.path.join(pcd_map_dir, "score.csv") + + if not os.path.exists(score_path): + print("Error: {0} does not exist!".format(score_path)) + exit() + + # Read the metadata file and get the list of segments + segment_df = pd.DataFrame(columns = ["x", "y", "tp"]) + + with open(metadata_path, "r") as f: + for key, value in yaml.safe_load(f).items(): + if key != "x_resolution" and key != "y_resolution": + seg_idx = name_to_coordinate(key) + segment_df.loc[len(segment_df)] = [seg_idx[0], seg_idx[1], 0] + + # Create a 2D kdtree on the segments + seg_tree_nodes = np.zeros((len(segment_df), 2), dtype = float) + + for index, row in segment_df.iterrows(): + seg_tree_nodes[index, :] = [row["x"], row["y"]] + + kdtree = sp.KDTree(seg_tree_nodes) + + # Read the score file + with open(score_path, "r") as f: + reader = csv.reader(f) + + for index, row in enumerate(reader): + segment_df.loc[index, "tp"] = float(row[1]) + + return {"segment_df" : segment_df, "kdtree" : kdtree} + +##### Stamp search ##### +def stamp_search(stamp: int, tp_df: pd.DataFrame) -> int: + left = 0 + right = len(tp_df) + res = -1 + + # Find the closest stamp that goes before the stamp key + while left < right: + mid = (left + right) // 2 + cur_stamp = tp_df["stamp"][mid] + + if cur_stamp <= stamp: + left = mid + 1 + res = mid + elif cur_stamp > stamp: + right = mid - 1 + + return res + +##### Read the input rosbag to obtain ndt pose and TP values ##### +def collect_rosbag_tp(bag_path: str) -> pd.DataFrame: + reader = SequentialReader() + bag_storage_options = StorageOptions(uri = bag_path, storage_id = "sqlite3") + bag_converter_options = ConverterOptions( + input_serialization_format = "cdr", output_serialization_format = "cdr" + ) + reader.open(bag_storage_options, bag_converter_options) + + pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) + tp_df = pd.DataFrame(columns = ["stamp", "tp"]) + + while reader.has_next(): + (topic, data, stamp) = reader.read_next() + + if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": + pose_msg = deserialize_message(data, PoseWithCovarianceStamped) + stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec + + pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] + elif topic == "/localization/pose_estimator/transform_probability": + tp_msg = deserialize_message(data, Float32Stamped) + stamp = tp_msg.header.stamp.sec * 1e9 + tp_msg.header.stamp.nanosec + + tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] + + # Now from the two list above build a table of estimated pose and corresponding TPs + ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp", "avg_tp"]) + + for row in pose_df.itertuples(): + stamp = row["stamp"] + pose = row["pose_msg"] + # Find the tp whose stamp is the closest to the pose + tid = stamp_search(stamp, tp_df) + + if tid >= 0: + ndt_res_df[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["data"][tid], 0.0] + + return ndt_res_df + +def estimate_tps(segment_dict: dict, ndt_res_df: pd.DataFrame): + for row in ndt_res_df.itertuples(): + nn_idx = segment_dict["kdtree"].query_ball_point([row["x"], row["y"]], 20.0) + sum_tp = 0.0 + + for idx in nn_idx: + sum_tp += segment_dict["segment_df"].loc[idx, "tp"] + + row["avg_tp"] = sum_tp / len(nn_idx) + +##### Save the tp and average tps to CSV file ##### +def save_results(ndt_res_df: pd.DataFrame, result_path: str): + ndt_res_df.to_csv(result_path) + +##### Show the results on 2D map ##### +def show(result_path: str): + print("Testing") + +def processing(pcd_map_dir: str, rosbag_path: str): + # Get the segment lists and scores + segment_dict = get_pcd_segments_and_scores(pcd_map_dir) + # Read the rosbag and get the ndt poses and corresponding tps + ndt_res_df = collect_rosbag_tp(rosbag_path) + # Update the TPs of segments + estimate_tps(segment_dict, ndt_res_df) + # Save the new TPs + result_path = os.path.join(pcd_map_dir, "result.csv") + save_results(ndt_res_df, result_path) + show(result_path) + + +if __name__ == "__main__": + 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") + + args = parser.parse_args() + + # Practice with string % a bit + print("Input PCD map at %s" % (args.map_path)) + print("Input rosbag at %s" % (args.bag_path)) + + # Run + processing(args.map_path, args.bag_path) diff --git a/map/autoware_tp_collector/scripts/tp_collector.py b/map/autoware_tp_collector/scripts/tp_collector.py index 9c194035..5ddaa034 100755 --- a/map/autoware_tp_collector/scripts/tp_collector.py +++ b/map/autoware_tp_collector/scripts/tp_collector.py @@ -27,15 +27,27 @@ ) from rclpy.serialization import deserialize_message, serialize_message -from subprocess import call, run, Popen import os -import time import csv import yaml -import shutil - -##### Get the list of PCD segments ##### -def get_pcd_segments(pcd_map_dir: str): +from scipy import spatial as sp +import numpy as np +from geometry_msgs.msg import PoseWithCovarianceStamped +from tier4_debug_msgs.msg import Float32Stamped +from builtin_interfaces.msg import Time +import pandas as pd + +##### Convert names of pcd segments to 2d coordinates ##### +def name_to_coordinate(seg_name: str): + # Remove the extension + name_only = os.path.splitext(seg_name)[0] + str_coors = name_only.split("_") + + # Return the coordinates as a 1x2 ndarray + return np.array(str_coors, dtype = float).reshape(1, 2) + +##### Read the YAML file to get the list of PCD segments and scores ##### +def get_pcd_segments_and_scores(pcd_map_dir: str) -> dict: if not os.path.exists(pcd_map_dir): print("Error: %s does not exist!"%(pcd_map_dir)) exit() @@ -46,321 +58,138 @@ def get_pcd_segments(pcd_map_dir: str): print("Error: %s does not exist!"%(pcd_path)) exit() - # Get list of PCD segments in the folder pointcloud_map.pcd - pcd_segment_list = [] + yaml_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") - for filename in os.listdir(pcd_path): - if filename.endswith(".pcd") or filename.endswith(".PCD"): - segment_path = os.path.join(pcd_path, filename) - pcd_segment_list.append(segment_path) + # Create a dataframe to record the avg tp of 2D segments + segment_df = pd.DataFrame(columns = ["x", "y", "tp"]) - + with open(yaml_path, "r") as f: + for key, value in yaml.safe_load(f).items(): + if key != "x_resolution" and key != "y_resolution": + seg_idx = name_to_coordinate(key) + segment_df.loc[len(segment_df)] = [seg_idx[0], seg_idx[1], 0] + # A 2D array that contains the 2D coordinates of segments + # We'll use this to build a KDtree + seg_tree_nodes = np.zeros((len(segment_df), 2), dtype = float) + for index, row in segment_df.iterrows(): + seg_tree_nodes[index,:] = [row["x"], row["y"]] + # Create a 2D kdtree on the segment list + kdtree = sp.KDTree(seg_tree_nodes) -##### Setup the working directory -def setup(work_dir: str): - if os.path.exists(work_dir): - if os.path.isdir(work_dir): - print("%s already exists. Do you want to overwrite its content? [y/n]"%(work_dir)) - else: - print("%s is not a directory. Do you want to delete it and create a new directory? [y/n]"%(work_dir)) - if input() != "y": - return - shutil.rmtree(work_dir) - os.mkdir(work_dir) - -##### Search for the topic list from the metadata -def topic_search(topic_list, metadata): - """ - This function iterates on each topic in the topic list, - and search for each topic from the metadata string. - :param topic_list: list of topic string - :param metadata: dict of {topic_name : number of messages} - """ - - output_list = [] + # Load the score map + score_path = os.path.join(pcd_map_dir, "scores.csv") - for topic in topic_list: - value = metadata.get(topic, "unknown") - if value != "unknown": - output_list.append(topic) - - return output_list - -###### Step 0: Check if some topics exist in the rosbag -# Print the metadata of the rosbag and look for the topics in the metadata -def topic_setup(topic_dict: dict, in_bag_dir: str): - # Setup lidar topic - lidar_topics_candidate = [ - # "/sensing/lidar/concatenated/pointcloud", - "/sensing/lidar/left_upper/pandar_packets", - # "/sensing/lidar/right_upper/pandar_packets", - # "/sensing/lidar/rear_upper/pandar_packets", - # "/sensing/lidar/right_lower/pandar_packets", - # "/sensing/lidar/left_lower/pandar_packets", - # "/sensing/lidar/front_lower/pandar_packets", - # "/sensing/lidar/front_upper/pandar_packets", - "/sensing/lidar/left_upper/pointcloud_raw_ex" - ] - # Setup GNSS topic - gnss_topic_candidate = ["/sensing/gnss/septentrio/nav_sat_fix", - "/sensing/gnss/ublox/nav_sat_fix", - "/sensing/gnss/nav_sat_fix"] - # Setup imu topic - imu_topic_candidate = ["/sensing/imu/imu_data", - "/sensing/imu/tamagawa/imu_raw"] - # Setup tf_static topic - tf_static_topic_candidate = ["/tf_static"] - # Setup velocity_status topic - velocity_status_topic_candidate = ["/vehicle/status/velocity_status"] - - # Setup a dictionary for rosbag metadata - bag_path = "" - for (dirpath, dirnames, filenames) in os.walk(in_bag_dir): - for fname in filenames: - if fname.endswith(".db3"): - bag_path = dirpath + "/" + fname - break - - info = Info() - metadata = info.read_metadata(bag_path, "sqlite3") - bag_topic_dict = {} - - for topic_info in metadata.topics_with_message_count: - bag_topic_dict[topic_info.topic_metadata.name] = topic_info.topic_metadata.type - - topic_dict["lidar"] = topic_search(lidar_topics_candidate, bag_topic_dict) - topic_dict["gnss"] = topic_search(gnss_topic_candidate, bag_topic_dict) - topic_dict["imu"] = topic_search(imu_topic_candidate, bag_topic_dict) - topic_dict["tf_static"] = topic_search(tf_static_topic_candidate, bag_topic_dict) - topic_dict["velocity_status"] = topic_search(velocity_status_topic_candidate, bag_topic_dict) - - print(topic_dict) + with open(score_path, "r") as f: + reader = csv.reader(f) + for index, row in enumerate(reader): + segment_df.loc[index, "tp"] = float(row[1]) -# Filter a rosbag to keep specified topcis only -def rosbag_filter(in_bag: str, out_bag: str, topic_mark: dict): + return {"segment_df": segment_df, "kdtree": kdtree} + +##### Stamp search ##### +def stamp_search(stamp: int, tp_df: pd.DataFrame) -> int: + left = 0 + right = len(tp_df) + res = -1 + + # Find the closest stamp that goes before the stamp key + while left < right: + mid = (left + right) // 2 + cur_stamp = tp_df["stamp"][mid] + + if cur_stamp <= stamp: + left = mid + 1 + res = mid + elif cur_stamp > stamp: + right = mid - 1 + + return res + +##### Read the input rosbag to obtain ndt pose and TP values ##### +def collect_rosbag_tp(bag_path: str) -> pd.DataFrame: reader = SequentialReader() - bag_storage_options = StorageOptions(uri = in_bag, storage_id = "sqlite3") - bag_converter_options = ConverterOptions(input_serialization_format = "cdr", output_serialization_format = "cdr") + bag_storage_options = StorageOptions(uri = bag_path, storage_id = "sqlite3") + bag_converter_options = ConverterOptions( + input_serialization_format = "cdr", output_serialization_format = "cdr" + ) reader.open(bag_storage_options, bag_converter_options) - writer = SequentialWriter() - bag_storage_options = StorageOptions(uri = out_bag, storage_id = "sqlite3") - bag_converter_options = ConverterOptions(input_serialization_format = "cdr", output_serialization_format = "cdr") - writer.open(bag_storage_options, bag_converter_options) - - all_topic_metadata = reader.get_all_topics_and_types() - - for topic_metadata in all_topic_metadata: - if topic_metadata.name in topic_mark: - writer.create_topic( - TopicMetadata( - name = topic_metadata.name, - type = topic_metadata.type, - serialization_format = "cdr" - ) - ) + pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) + tp_df = pd.DataFrame(columns = ["stamp", "tp"]) while reader.has_next(): (topic, data, stamp) = reader.read_next() - if topic in topic_mark: - writer.write(topic, data, stamp) - -# Step 1: Pre-processing the input rosbags -# Remove unused topics and merge small rosbags to a big one -def preprocessing(topic_dict: dict, in_bag_dir: str, work_dir: str): - """ - Filtering rosbag to extract necessary topics only, also - generate a metadata.yaml for the filtered rosbag. - :param topic_dict: dict, a dictionary of required topics from the input rosbag - :param in_bag_dir: str, path to the input rosbag - :param work_dir: str, path to the working directory, which contains all temporal and output data - """ - # If the merged rosbag already exist, quit - merged_bag = os.path.normpath(work_dir + "/merged.db3") - if os.path.exists(merged_bag): - print("Merged rosbag already exists at %s. Do you want to re-create? [y/n]"%(merged_bag)) - if input() != "y": - return - - # Scan the input bag directory and get all files having the .db3 extension - bag_list = [] - filtered_bag_list = [] - for (dirpath, dirnames, filenames) in os.walk(in_bag_dir): - for fname in filenames: - if fname.endswith(".db3"): - bag_list.append(dirpath + fname) - - bag_list.sort() - - topic_set = set() + if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": + pose_msg = deserialize_message(data, PoseWithCovarianceStamped) + stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec - for (topic_tag, topic_names) in topic_dict.items(): - for name in topic_names: - print("Adding topic %s"%(name)) - topic_set.add(name) + pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] + elif topic == "/localization/pose_estimator/transform_probability": + tp_msg = deserialize_message(data, Float32Stamped) + stamp = tp_msg.header.stamp.sec * 1e9 + tp_msg.header.stamp.nanosec - # Filter every input rosbag - for in_bag in bag_list: - filtered_in_bag = work_dir + "/" + os.path.splitext(os.path.basename(in_bag))[0] + "_filtered.db3" - rosbag_filter(in_bag, filtered_in_bag, topic_set) - filtered_bag_list.append(filtered_in_bag) - - # Merge all filtered rosbag to a single one - merge_cmd = "ros2 bag merge -o " + merged_bag - for filtered_in_bag in filtered_bag_list: - merge_cmd += " " + filtered_in_bag - - call(merge_cmd, shell = True) - - # Delete all filtered rosbags, only keep the merged rosbag - for filtered_in_bag in filtered_bag_list: - call("rm -fr " + filtered_in_bag, shell = True) - -# Step 2: compare the rosbag with the PCD map and detect changed areas -def map_change_checking(topic_dict: dict, work_dir: str, map_path: str, m4e_mount_path: str): - """ - Compare the filtered rosbag with the PCD map, and detect map areas that changed. - Filter the filtered rosbag to extract scans that cover the changed areas only, - and remove invalid GNSS messages from the filtered rosbag. Convert the remaining - of the filtered rosbag to the ROS1 format. - """ - # Generate rosbag for evaluation - eval_bag = os.path.normpath(work_dir + "/eval_bag") - vehicle_model = "j6_gen1" - sensor_model = "aip_x2" - - # Launch pilot-auto.x2's logging simulator - # Popen(["ros2", "launch", "autoware_launch", "logging_simulator.launch.xml", \ - # "map_path:=" + map_path, "vehicle_model:=j6_gen1", "sensor_model:=aip_x2"]) - # # ,\ - # # "perception:=false", "planning:=false", "control:=false"]) - # Wait for the logging simulator finishes setting up - # time.sleep(360) - # record_proc = Popen(["ros2", "bag", "record", "-o", eval_bag, "/localization/pose_twist_fusion_filter/biased_pose_with_covariance", \ - # "/localization/util/downsample/pointcloud"]) - # ros2 bag record -o eval_bag /localization/pose_twist_fusion_filter/biased_pose_with_covariance /localization/util/downsample/pointcloud + tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] - # Play the rosbag and wait for it to finish - # call("ros2 bag play " + work_dir + "/merged.db3 -r 2.0 -s sqlite3 --clock 100", shell = True) - - # Quit - # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -2 $(pgrep -P \", $1, \") > /dev/null 2>&1\" }' | sh") - # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -2 \", $1, \" > /dev/null 2>&1\" }' | sh") - # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -9 $(pgrep -P \", $1, \") > /dev/null 2>&1\" }' | sh") - # os.system("/usr/bin/pgrep ros | /usr/bin/awk '{ print \"kill -9 \", $1, \" > /dev/null 2>&1\" }' | sh") - - # # Check if the eval rosbag exists - # if not(os.path.exists(eval_bag)): - # print("Evaluation rosbag does not exist at %s. Exit..."%(eval_bag)) - # exit() - # else: - # print("Evaluation rosbag exists at %s."%(eval_bag)) - - # # Run map_assessment_tools - # mast_output = os.path.normpath(work_dir + "/mast_result/") - - # mast_cmd = "ros2 launch map_assessment_tools map_evaluation.launch.py rosbag_file_name:=" + \ - # eval_bag + " map_path:=" + map_path + " save_dir:=" + mast_output + \ - # " sensor_topic_name:=/localization/util/downsample/pointcloud roc:=true" - - # if not(os.path.exists(mast_output)): - # os.mkdir(mast_output) - # call(mast_cmd, shell = True) - # else: - # print("map_assessment_tools output already exists. Do you want to re-run? [y/n]") - # if input() == "y": - # call(mast_cmd, shell = True) - - # Filter the merged bag to keep the scans that cover the changed area - # Also remove invalid GNSS messages - final_bag = os.path.normpath(work_dir + "/final_bag.db3") - filter_cmd = "ros2 run map_assessment_tools rosbag_interval_filter.py " + \ - work_dir + "/merged.db3 " + final_bag + \ - " --gnss_topic " + ",".join(topic_dict["gnss"]) + \ - " --sensor_topic " + ",".join(topic_dict["lidar"]) - # " --save_dir " + mast_output + \ - # " --gnss_topic " + topic_dict["gnss"] + \ - # " --sensor_topic " + topic_dict["lidar"] - - if os.path.exists(final_bag): - print("The filtered final ROS2 rosbag already exists. Do you want to re-filter? [y/n]") - if input() == "y": - shutil.rmtree(final_bag) - call(filter_cmd, shell = True) - else: - call(filter_cmd, shell = True) - - # Convert the final rosbag from ROS2 format to ROS1 format - print("Convert to ROS1 format...") - call("/home/anh/Work/rosbags/venv/bin/rosbags-convert-2to1 " + final_bag, shell = True) - # Move the ROS1 bag to map4-cli output path - m4e_path = os.path.normpath(m4e_mount_path + "/" + os.path.basename(work_dir) + "/") - if os.path.exists(m4e_path): - print("map4-cli mount path already exists at %s. Do you want to re-create? [y/n]"%(m4e_path)) - if input() == "y": - shutil.rmtree(m4e_path) - os.mkdir(m4e_path) - call("mv " + final_bag + ".bag " + m4e_path + "/input_rosbag.bag", shell = True) - print("A ROS1 rosbag was copied to %s."%(m4e_path + "/input_rosbag.bag")) - else: - os.mkdir(m4e_path) - call("mv " + final_bag + ".bag " + m4e_path + "/input_rosbag.bag", shell = True) - print("A ROS1 rosbag was copied to %s."%(m4e_path + "/input_rosbag.bag")) - - # Merge pcd files and put them to m4e_path/map - m4e_map_path = os.path.normpath(m4e_path + "/map") - if os.path.exists(m4e_map_path): - print("The map folder for PCD update already exists at %s. Do you want to re-create? [y/n]"%(m4e_map_path)) - if input() == "y": - shutil.rmtree(m4e_map_path) - os.mkdir(m4e_map_path) - else: - os.mkdir(m4e_map_path) - - merge_cmd = "ros2 launch autoware_pointcloud_merger pointcloud_merger.launch.xml " + \ - "input_pcd_dir:=" + map_path + "/pointcloud_map.pcd/" + \ - " output_pcd:=" + m4e_map_path + "/merged.pcd" - - call(merge_cmd, shell = True) - print("PCD Update preparation is done.") + # Now from the two list above build a table of estimated pose and corresponding TPs + ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp"]) + + for row in pose_df.itertuples(): + stamp = row["stamp"] + pose = row["pose_msg"] + # Find the tp whose stamp is the closest to the pose + tid = stamp_search(stamp, tp_df) + + if tid >= 0: + ndt_res_df[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["data"][tid]] + + return ndt_res_df + +##### Update map's TP ##### +def update_avg_tp(ndt_res_df: pd.DataFrame, segment_dict: dict): + # Iterate on poses and the corresponding TPs + for tp_row in ndt_res_df.itertuples(): + # Find indices of map segments that cover the current pose + nn_idx = segment_dict["kdtree"].query_ball_point([tp_row["x"], tp_row["y"]], 20.0) + + for idx in nn_idx: + # If the TP from rosbag is greater than the current TP of the segment, + # replace the segment TP with the TP from the rosbag + # TODO: record the average TP of segments + if tp_row[1] > segment_dict["segment_df"].loc[idx, "tp"]: + segment_dict["segment_df"].loc[idx, "tp"] = tp_row[1] + +##### Save the segment TPs ##### +def save_tps(pcd_map_dir: str, segment_dict: dict): + score_path = os.path.join(pcd_map_dir, "score.csv") + + with open(score_path, "w") as f: + for row in segment_dict["segment_df"].itertuples(): + f.write("%d_%d,%f\n" % (int(row["x"]), int(row["y"]), row["tp"])) + +def processing(pcd_map_dir: str, rosbag_path: str): + # Get the segment lists and scores + segment_dict = get_pcd_segments_and_scores(pcd_map_dir) + # Read the rosbag and get the ndt poses and corresponding tps + ndt_res_df = collect_rosbag_tp(rosbag_path) + # Update the TPs of segments + update_avg_tp(ndt_res_df, segment_dict) + # Save the new TPs + save_tps(pcd_map_dir, segment_dict) if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument("map_path", help="The path to the folder containing PCD files of the old map") + 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("output_path", help="The path to the folder containing the updated PCD files") - parser.add_argument("--type", help="Type of launcher pilot-auto.x2/pilot-auto.xx1", default="pilot-auto.x2", required=False) args = parser.parse_args() # Practice with string % a bit print("Input PCD map at %s" % (args.map_path)) print("Input rosbag at %s" % (args.bag_path)) - print("Output PCD map at %s" % (args.output_path)) - - # Step 0: Setup required topics - # To evaluate and update map, the filtered rosbag must contain - # IMU, GNSS, Lidar, TF Static, and velocity status - topic_dict = { - "lidar": "unknown", - "gnss": "unknown", - "imu": "unknown", - "tf_static": "unknown", - "velocity_status": "unknown", - } - # Cleanup the working directory - work_dir = os.path.normpath(args.output_path) - setup(work_dir) - # Setup topics for filtering and merge - topic_setup(topic_dict, args.bag_path) - # Step 1: Pre-process rosbag - preprocessing(topic_dict, args.bag_path, work_dir) - # Step 2: Check map changes - m4e_mount_path = os.path.normpath("/media/anh/AnhNguyen/map4_engine_output/") - map_change_checking(topic_dict, work_dir, args.map_path, m4e_mount_path) - # Step 3: Update PCD map - # Step 4: Post validation + + # Run + processing(args.map_path, args.bag_path) diff --git a/map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp b/map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp deleted file mode 100644 index 2cecbb45..00000000 --- a/map/autoware_tp_collector/src/include/pointcloud_merger_node.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// 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. - -#ifndef POINTCLOUD_MERGER_NODE_HPP_ -#define POINTCLOUD_MERGER_NODE_HPP_ - -#include - -#define PCL_NO_RECOMPILE -#include - -namespace autoware::pointcloud_merger -{ - -class PointCloudMerger : public rclcpp::Node -{ -public: - explicit PointCloudMerger(const rclcpp::NodeOptions & node_options); -}; - -} // namespace autoware::pointcloud_merger - -#endif // POINTCLOUD_MERGER_NODE_HPP_ diff --git a/map/autoware_tp_collector/src/pcd_merger.cpp b/map/autoware_tp_collector/src/pcd_merger.cpp deleted file mode 100644 index d4cf5869..00000000 --- a/map/autoware_tp_collector/src/pcd_merger.cpp +++ /dev/null @@ -1,221 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// 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. - -// BSD 3-Clause License -// -// Copyright (c) 2023, MAP IV -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include "include/pointcloud_merger_node.hpp" - -#include -#include -#include - -#include - -#include -#include -#include - -namespace fs = std::filesystem; - -namespace autoware::pointcloud_merger -{ - -template -std::vector PCDMerger::discoverPCDs(const std::string & input) -{ - fs::path input_path(input); - - if (!fs::is_directory(input_path)) { - RCLCPP_ERROR(logger_, "Error: Invalid input directory %s", input.c_str()); - rclcpp::shutdown(); - } - - std::vector pcd_list; - - for (auto & entry : fs::directory_iterator(input_path)) { - if (fs::is_regular_file(entry.symlink_status())) { - auto file_name = entry.path().string(); - auto extension = entry.path().extension().string(); - - if (extension == ".pcd" || extension == ".PCD") { - pcd_list.push_back(file_name); - } - } - } - - RCLCPP_INFO(logger_, "Found %lu PCD files", pcd_list.size()); - - return pcd_list; -} - -template -void PCDMerger::run() -{ - auto pcd_list = discoverPCDs(input_dir_); - - run(pcd_list); -} - -template -void PCDMerger::run(const std::vector & pcd_names) -{ - // Just in case the downsampling option is on - if (leaf_size_ > 0) { - tmp_dir_ = "./pointcloud_merger_tmp/"; - - if (fs::exists(tmp_dir_)) { - fs::remove_all(tmp_dir_); - } - - autoware::pointcloud_divider::util::make_dir(tmp_dir_); - } - - if (fs::exists(output_pcd_)) { - fs::remove_all(output_pcd_); - } - - if (leaf_size_ > 0) { - mergeWithDownsample(pcd_names); - autoware::pointcloud_divider::util::remove(tmp_dir_); - } else { - mergeWithoutDownsample(pcd_names); - } -} - -template -void PCDMerger::mergeWithDownsample(const std::vector & input_pcds) -{ - RCLCPP_INFO(logger_, "Downsampling by PointCloudDivider"); - // Divide the input clouds to smaller segments - autoware::pointcloud_divider::PCDDivider pcd_divider(logger_); - - pcd_divider.setOutputDir(tmp_dir_); - pcd_divider.setGridSize(leaf_size_ * 100, leaf_size_ * 100); - pcd_divider.setPrefix("tmp_segment"); - pcd_divider.setLeafSize(leaf_size_); - pcd_divider.setDebugMode(false); - pcd_divider.run(input_pcds); - - // Now go get path of the segmentation pcd - std::vector seg_names; - fs::path tmp_path(tmp_dir_); - - for (auto & entry : fs::recursive_directory_iterator(tmp_path)) { - if (fs::is_regular_file(entry.symlink_status())) { - auto fname = entry.path().string(); - auto ext = fname.substr(fname.size() - 4); - - if (ext == ".pcd") { - seg_names.push_back(fname); - } - } - } - - // Now merge the downsampled segments - mergeWithoutDownsample(seg_names); -} - -template -void PCDMerger::mergeWithoutDownsample(const std::vector & input_pcds) -{ - if (input_pcds.size() == 0) { - RCLCPP_INFO(logger_, "No input PCDs. Return!"); - - return; - } - - // Check the number of points of the merger - size_t total_point_num = 0; - autoware::pointcloud_divider::CustomPCDReader reader; - - for (const auto & pcd_name : input_pcds) { - reader.setInput(pcd_name); - - total_point_num += reader.point_num(); - } - - writer_.setOutput(output_pcd_); - writer_.writeMetadata(total_point_num, true); - - size_t file_counter = 0; - - for (const auto & pcd_name : input_pcds) { - if (!rclcpp::ok()) { - return; - } - - RCLCPP_INFO( - logger_, "Processing file [%lu/%lu] %s", file_counter, input_pcds.size(), pcd_name.c_str()); - ++file_counter; - - reader.setInput(pcd_name); - - do { - PclCloudType new_cloud; - - reader.readABlock(new_cloud); - writer_.write(new_cloud); - } while (reader.good() && rclcpp::ok()); - } -} - -template -void PCDMerger::paramInitialize() -{ - try { - YAML::Node conf = YAML::LoadFile(config_file_); - auto params = conf["/**"]["ros__parameters"]; - - leaf_size_ = params["leaf_size"].as(); - } catch (YAML::Exception & e) { - RCLCPP_ERROR(logger_, "YAML Error: %s", e.what()); - rclcpp::shutdown(); - exit(EXIT_FAILURE); - } -} - -template class PCDMerger; -template class PCDMerger; -// template class PCDMerger; -// template class PCDMerger; -// template class PCDMerger; - -} // namespace autoware::pointcloud_merger diff --git a/map/autoware_tp_collector/src/pointcloud_merger_node.cpp b/map/autoware_tp_collector/src/pointcloud_merger_node.cpp deleted file mode 100644 index 4a720703..00000000 --- a/map/autoware_tp_collector/src/pointcloud_merger_node.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// 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. - -#include "include/pointcloud_merger_node.hpp" - -#include - -#include - -#include - -namespace autoware::pointcloud_merger -{ - -PointCloudMerger::PointCloudMerger(const rclcpp::NodeOptions & node_options) -: Node("pointcloud_merger", node_options) -{ - // Load command parameters - float leaf_size = declare_parameter("leaf_size"); - std::string input_pcd_dir = declare_parameter("input_pcd_dir"); - std::string output_pcd = declare_parameter("output_pcd"); - std::string point_type = declare_parameter("point_type"); - - // Enter a new line and clear it - // This is to get rid of the prefix of RCLCPP_INFO - std::string line_breaker(102, ' '); - - line_breaker[0] = '\n'; - line_breaker[1] = line_breaker[101] = '\r'; - - std::ostringstream param_display; - - param_display << line_breaker << line_breaker << "########## Input Parameters ##########" - << line_breaker; - - param_display << "\tleaf_size: " << leaf_size << line_breaker; - param_display << "\tinput_pcd_dir: " << input_pcd_dir << line_breaker; - param_display << "\toutput_pcd: " << output_pcd << line_breaker; - param_display << "\tpoint_type: " << point_type << line_breaker; - param_display << "######################################" << line_breaker; - - RCLCPP_INFO(get_logger(), "%s", param_display.str().c_str()); - - if (point_type == "point_xyz") { - autoware::pointcloud_merger::PCDMerger pcd_merger_exe(get_logger()); - - pcd_merger_exe.setLeafSize(leaf_size); - pcd_merger_exe.setInput(input_pcd_dir); - pcd_merger_exe.setOutput(output_pcd); - - pcd_merger_exe.run(); - } else if (point_type == "point_xyzi") { - autoware::pointcloud_merger::PCDMerger pcd_merger_exe(get_logger()); - - pcd_merger_exe.setLeafSize(leaf_size); - pcd_merger_exe.setInput(input_pcd_dir); - pcd_merger_exe.setOutput(output_pcd); - - pcd_merger_exe.run(); - } - - rclcpp::shutdown(); -} - -} // namespace autoware::pointcloud_merger - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_merger::PointCloudMerger) From 1431faa6eaa55e972aa840d8d47ad4bf891e318c Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Mon, 20 Jan 2025 10:27:29 +0700 Subject: [PATCH 3/7] TP manager can collect tp per segments now Signed-off-by: Anh Nguyen --- .../scripts/tp_collector.py | 66 ++++++++++++++----- 1 file changed, 51 insertions(+), 15 deletions(-) diff --git a/map/autoware_tp_collector/scripts/tp_collector.py b/map/autoware_tp_collector/scripts/tp_collector.py index 5ddaa034..f8629d2d 100755 --- a/map/autoware_tp_collector/scripts/tp_collector.py +++ b/map/autoware_tp_collector/scripts/tp_collector.py @@ -36,6 +36,23 @@ from tier4_debug_msgs.msg import Float32Stamped from builtin_interfaces.msg import Time import pandas as pd +import tqdm + + +def compute_total_message_count(rosbag_path): + if rosbag_path.endswith(".db3"): + yaml_path = os.path.join(os.path.dirname(rosbag_path), "metadata.yaml") + else: + yaml_path = os.path.join(rosbag_path, "metadata.yaml") + + with open(yaml_path, "r") as f: + data = yaml.safe_load(f) + + total_message_count = 0 + for topic_data in data["rosbag2_bagfile_information"]["topics_with_message_count"]: + total_message_count += topic_data["message_count"] + + return total_message_count ##### Convert names of pcd segments to 2d coordinates ##### def name_to_coordinate(seg_name: str): @@ -66,8 +83,7 @@ def get_pcd_segments_and_scores(pcd_map_dir: str) -> dict: with open(yaml_path, "r") as f: for key, value in yaml.safe_load(f).items(): if key != "x_resolution" and key != "y_resolution": - seg_idx = name_to_coordinate(key) - segment_df.loc[len(segment_df)] = [seg_idx[0], seg_idx[1], 0] + segment_df.loc[len(segment_df)] = [float(value[0]), float(value[1]), 0] # A 2D array that contains the 2D coordinates of segments # We'll use this to build a KDtree @@ -82,10 +98,14 @@ def get_pcd_segments_and_scores(pcd_map_dir: str) -> dict: # Load the score map score_path = os.path.join(pcd_map_dir, "scores.csv") - with open(score_path, "r") as f: - reader = csv.reader(f) - for index, row in enumerate(reader): - segment_df.loc[index, "tp"] = float(row[1]) + if os.path.exists(score_path): + with open(score_path, "r") as f: + reader = csv.reader(f) + for index, row in enumerate(reader): + segment_df.loc[index, "tp"] = float(row[1]) + else: + # If the score file does not exist, initialize scores to all 0 + segment_df.loc[:,"tp"] = 0 return {"segment_df": segment_df, "kdtree": kdtree} @@ -117,62 +137,78 @@ def collect_rosbag_tp(bag_path: str) -> pd.DataFrame: ) reader.open(bag_storage_options, bag_converter_options) + total_message_count = compute_total_message_count(bag_path) + progress_bar = tqdm.tqdm(total=total_message_count) + + pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) tp_df = pd.DataFrame(columns = ["stamp", "tp"]) while reader.has_next(): + progress_bar.update(1) (topic, data, stamp) = reader.read_next() if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": pose_msg = deserialize_message(data, PoseWithCovarianceStamped) stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec - pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] elif topic == "/localization/pose_estimator/transform_probability": tp_msg = deserialize_message(data, Float32Stamped) - stamp = tp_msg.header.stamp.sec * 1e9 + tp_msg.header.stamp.nanosec + stamp = tp_msg.stamp.sec * 1e9 + tp_msg.stamp.nanosec tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] # Now from the two list above build a table of estimated pose and corresponding TPs ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp"]) + progress_bar = tqdm.tqdm(total=len(pose_df)) + for row in pose_df.itertuples(): - stamp = row["stamp"] - pose = row["pose_msg"] + progress_bar.update(1) + stamp = row.stamp + pose = row.pose_msg # Find the tp whose stamp is the closest to the pose tid = stamp_search(stamp, tp_df) if tid >= 0: - ndt_res_df[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["data"][tid]] + ndt_res_df.loc[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid]] return ndt_res_df ##### Update map's TP ##### def update_avg_tp(ndt_res_df: pd.DataFrame, segment_dict: dict): + print("UPDATE AVG TP, len of ndt_res_df = {0}".format(len(ndt_res_df))) # Iterate on poses and the corresponding TPs + progress_bar = tqdm.tqdm(total=len(ndt_res_df)) + for tp_row in ndt_res_df.itertuples(): # Find indices of map segments that cover the current pose - nn_idx = segment_dict["kdtree"].query_ball_point([tp_row["x"], tp_row["y"]], 20.0) + nn_idx = segment_dict["kdtree"].query_ball_point([tp_row.x, tp_row.y], 20.0) for idx in nn_idx: # If the TP from rosbag is greater than the current TP of the segment, # replace the segment TP with the TP from the rosbag # TODO: record the average TP of segments - if tp_row[1] > segment_dict["segment_df"].loc[idx, "tp"]: - segment_dict["segment_df"].loc[idx, "tp"] = tp_row[1] + if tp_row.tp > segment_dict["segment_df"].loc[idx, "tp"]: + segment_dict["segment_df"].loc[idx, "tp"] = tp_row.tp ##### Save the segment TPs ##### def save_tps(pcd_map_dir: str, segment_dict: dict): + print("SAVE AVG TP") + score_path = os.path.join(pcd_map_dir, "score.csv") + print(segment_dict) + with open(score_path, "w") as f: for row in segment_dict["segment_df"].itertuples(): - f.write("%d_%d,%f\n" % (int(row["x"]), int(row["y"]), row["tp"])) + f.write("{0}_{1},{2}\n".format(int(row.x), int(row.y), row.tp)) def processing(pcd_map_dir: str, rosbag_path: str): # Get the segment lists and scores segment_dict = get_pcd_segments_and_scores(pcd_map_dir) + print("PAUSE at 179 segment dict is \n{0}".format(segment_dict)) + # Read the rosbag and get the ndt poses and corresponding tps ndt_res_df = collect_rosbag_tp(rosbag_path) # Update the TPs of segments From b95f00b1733446134c7aaf7d50a142a74d0e2336 Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Thu, 23 Jan 2025 11:01:57 +0700 Subject: [PATCH 4/7] Added a tool to check the TP changes in comparison with the map's TP Signed-off-by: Anh Nguyen --- .../scripts/tp_checker.py | 366 +++++++++++------- .../scripts/tp_collector.py | 9 - 2 files changed, 225 insertions(+), 150 deletions(-) diff --git a/map/autoware_tp_collector/scripts/tp_checker.py b/map/autoware_tp_collector/scripts/tp_checker.py index 67953e4c..8831f418 100755 --- a/map/autoware_tp_collector/scripts/tp_checker.py +++ b/map/autoware_tp_collector/scripts/tp_checker.py @@ -36,155 +36,236 @@ from tier4_debug_msgs.msg import Float32Stamped from builtin_interfaces.msg import Time import pandas as pd - -##### Convert names of pcd segments to 2d numpy array ##### -def name_to_coordinate(seg_name: str): - # Remove the extension - name_only = os.path.splitext(seg_name)[0] - str_coors = name_only.split("_") - - # Return the coordinates as a 1x2 ndarray - return np.array(str_coors, dtype = float).reshape(1, 2) - -def get_pcd_segments_and_scores(pcd_map_dir: str) -> pd.DataFrame: - # Check if the required folders/files exist - if not os.path.exists(pcd_map_dir): - print("Error: {0} does not exist!".format(pcd_map_dir)) - exit() - - pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd") - - if not os.path.exists(pcd_path): - print("Error: {0} does not exist!".format(pcd_path)) - exit() - - metadata_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") - - if not os.path.exists(metadata_path): - print("Error: {0} does not exist!".format(metadata_path)) - exit() - - score_path = os.path.join(pcd_map_dir, "score.csv") - - if not os.path.exists(score_path): - print("Error: {0} does not exist!".format(score_path)) - exit() - - # Read the metadata file and get the list of segments - segment_df = pd.DataFrame(columns = ["x", "y", "tp"]) - - with open(metadata_path, "r") as f: - for key, value in yaml.safe_load(f).items(): - if key != "x_resolution" and key != "y_resolution": - seg_idx = name_to_coordinate(key) - segment_df.loc[len(segment_df)] = [seg_idx[0], seg_idx[1], 0] - - # Create a 2D kdtree on the segments - seg_tree_nodes = np.zeros((len(segment_df), 2), dtype = float) - - for index, row in segment_df.iterrows(): - seg_tree_nodes[index, :] = [row["x"], row["y"]] - - kdtree = sp.KDTree(seg_tree_nodes) - - # Read the score file - with open(score_path, "r") as f: - reader = csv.reader(f) - - for index, row in enumerate(reader): - segment_df.loc[index, "tp"] = float(row[1]) - - return {"segment_df" : segment_df, "kdtree" : kdtree} - -##### Stamp search ##### -def stamp_search(stamp: int, tp_df: pd.DataFrame) -> int: - left = 0 - right = len(tp_df) - res = -1 - - # Find the closest stamp that goes before the stamp key - while left < right: - mid = (left + right) // 2 - cur_stamp = tp_df["stamp"][mid] - - if cur_stamp <= stamp: - left = mid + 1 - res = mid - elif cur_stamp > stamp: - right = mid - 1 - - return res - -##### Read the input rosbag to obtain ndt pose and TP values ##### -def collect_rosbag_tp(bag_path: str) -> pd.DataFrame: - reader = SequentialReader() - bag_storage_options = StorageOptions(uri = bag_path, storage_id = "sqlite3") - bag_converter_options = ConverterOptions( - input_serialization_format = "cdr", output_serialization_format = "cdr" - ) - reader.open(bag_storage_options, bag_converter_options) - - pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) - tp_df = pd.DataFrame(columns = ["stamp", "tp"]) - - while reader.has_next(): - (topic, data, stamp) = reader.read_next() - - if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": - pose_msg = deserialize_message(data, PoseWithCovarianceStamped) - stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec - - pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] - elif topic == "/localization/pose_estimator/transform_probability": - tp_msg = deserialize_message(data, Float32Stamped) - stamp = tp_msg.header.stamp.sec * 1e9 + tp_msg.header.stamp.nanosec - - tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] - - # Now from the two list above build a table of estimated pose and corresponding TPs - ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp", "avg_tp"]) - - for row in pose_df.itertuples(): - stamp = row["stamp"] - pose = row["pose_msg"] - # Find the tp whose stamp is the closest to the pose - tid = stamp_search(stamp, tp_df) +import open3d as o3d +import matplotlib.pyplot as plt +import tqdm + +def compute_total_message_count(rosbag_path): + if rosbag_path.endswith(".db3"): + yaml_path = os.path.join(os.path.dirname(rosbag_path), "metadata.yaml") + else: + yaml_path = os.path.join(rosbag_path, "metadata.yaml") + + with open(yaml_path, "r") as f: + data = yaml.safe_load(f) + + total_message_count = 0 + for topic_data in data["rosbag2_bagfile_information"]["topics_with_message_count"]: + total_message_count += topic_data["message_count"] + + return total_message_count + +class TPChecker: + def __init__(self): + self.segment_dict = {} # Segment indices, TP, kdtree + self.base_path = "" # Path to the base directory of the PCD segments + self.ndt_res_df = pd.DataFrame() # A set of tp read from a rosbag + self.pcd_map_2d = pd.DataFrame(columns = ["x", "y", "color"]) + # A 2D map of pcd, color indicates the map changes + self.point_num = 0 + + # Read the input map directory and setup the segment dictionary + def set_pcd_map(self, pcd_map_dir: str): + # Check if the required folders/files exist + if not os.path.exists(pcd_map_dir): + print("Error: {0} does not exist!".format(pcd_map_dir)) + exit() - if tid >= 0: - ndt_res_df[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["data"][tid], 0.0] + pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd") + + if not os.path.exists(pcd_path): + print("Error: {0} does not exist!".format(pcd_path)) + exit() - return ndt_res_df + metadata_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") + + if not os.path.exists(metadata_path): + print("Error: {0} does not exist!".format(metadata_path)) + exit() + + score_path = os.path.join(pcd_map_dir, "score.csv") + + if not os.path.exists(score_path): + print("Error: {0} does not exist!".format(score_path)) + exit() + + # Read the metadata file and get the list of segments + segment_df = pd.DataFrame(columns = ["x", "y", "tp", "pcd"]) -def estimate_tps(segment_dict: dict, ndt_res_df: pd.DataFrame): - for row in ndt_res_df.itertuples(): - nn_idx = segment_dict["kdtree"].query_ball_point([row["x"], row["y"]], 20.0) - sum_tp = 0.0 + with open(metadata_path, "r") as f: + for key, value in yaml.safe_load(f).items(): + if key != "x_resolution" and key != "y_resolution": + # Read PCD files and store them into a 2D map + seg_path = os.path.join(pcd_path, key) + pcd = o3d.io.read_point_cloud(seg_path) + # Downsample the segment to a greater resolution + # This segment is just for visualizing the result, no need to be detailed + ds_pcd = self.__downsample(np.asarray(pcd.points)) + segment_df.loc[len(segment_df)] = [float(value[0]), float(value[1]), 0, ds_pcd] + self.point_num += len(ds_pcd) - for idx in nn_idx: - sum_tp += segment_dict["segment_df"].loc[idx, "tp"] + # Create a 2D kdtree on the segments + seg_tree_nodes = np.zeros((len(segment_df), 2), dtype = float) + + for index, row in segment_df.iterrows(): + seg_tree_nodes[index, :] = [row["x"], row["y"]] + + kdtree = sp.KDTree(seg_tree_nodes) + + # Read the score file + with open(score_path, "r") as f: + reader = csv.reader(f) + + for index, row in enumerate(reader): + segment_df.loc[index, "tp"] = float(row[1]) + + self.segment_dict["segment_df"] = segment_df + self.segment_dict["kdtree"] = kdtree + self.base_path = pcd_path + + ##### Stamp search ##### + def __stamp_search(self, stamp: int, tp_df: pd.DataFrame) -> int: + left = 0 + right = len(tp_df) + res = -1 + + # Find the closest stamp that goes before the stamp key + while left < right: + mid = (left + right) // 2 + cur_stamp = tp_df["stamp"][mid] + + if cur_stamp <= stamp: + left = mid + 1 + res = mid + elif cur_stamp > stamp: + right = mid - 1 + + return res + + ##### Read the input rosbag to obtain ndt pose and TP values ##### + def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: + reader = SequentialReader() + bag_storage_options = StorageOptions(uri = bag_path, storage_id = "sqlite3") + bag_converter_options = ConverterOptions( + input_serialization_format = "cdr", output_serialization_format = "cdr" + ) + reader.open(bag_storage_options, bag_converter_options) + + total_message_count = compute_total_message_count(bag_path) + progress_bar = tqdm.tqdm(total=total_message_count) + + pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) + tp_df = pd.DataFrame(columns = ["stamp", "tp"]) + + while reader.has_next(): + progress_bar.update(1) + (topic, data, stamp) = reader.read_next() + + if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": + pose_msg = deserialize_message(data, PoseWithCovarianceStamped) + stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec + + pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] + elif topic == "/localization/pose_estimator/transform_probability": + tp_msg = deserialize_message(data, Float32Stamped) + stamp = tp_msg.stamp.sec * 1e9 + tp_msg.stamp.nanosec + + tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] - row["avg_tp"] = sum_tp / len(nn_idx) + # Now from the two list above build a table of estimated pose and corresponding TPs + self.ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp", "avg_tp"]) -##### Save the tp and average tps to CSV file ##### -def save_results(ndt_res_df: pd.DataFrame, result_path: str): - ndt_res_df.to_csv(result_path) + progress_bar = tqdm.tqdm(total=len(pose_df)) -##### Show the results on 2D map ##### -def show(result_path: str): - print("Testing") + for row in pose_df.itertuples(): + progress_bar.update(1) -def processing(pcd_map_dir: str, rosbag_path: str): - # Get the segment lists and scores - segment_dict = get_pcd_segments_and_scores(pcd_map_dir) - # Read the rosbag and get the ndt poses and corresponding tps - ndt_res_df = collect_rosbag_tp(rosbag_path) - # Update the TPs of segments - estimate_tps(segment_dict, ndt_res_df) - # Save the new TPs - result_path = os.path.join(pcd_map_dir, "result.csv") - save_results(ndt_res_df, result_path) - show(result_path) + stamp = row.stamp + pose = row.pose_msg + # Find the tp whose stamp is the closest to the pose + tid = self.__stamp_search(stamp, tp_df) + + if tid >= 0: + self.ndt_res_df[len(self.ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid], 0.0] + # Mark the segments which may has changed + def __mark_changes(self): + print("Checking map changes...\n") + change_mark = np.zeros(len(self.segment_dict["segment_df"]), dtype = int) + + progress_bar = tqdm.tqdm(total = len(self.ndt_res_df)) + + for row in self.ndt_res_df.itertuples(): + progress_bar.update(1) + nn_idx = self.segment_dict["kdtree"].query_ball_point([row.x, row.y], 20.0) + + if len(nn_idx) == 0: + continue + + sum_tp = 0.0 + + for idx in nn_idx: + sum_tp += self.segment_dict["segment_df"].loc[idx, "tp"] + + row.avg_tp = sum_tp / len(nn_idx) + + if abs(row.tp - row.avg_tp) / row.tp >= 0.2: + for idx in nn_idx: + change_mark[idx] = 1 + + # Export the 2D map + print("Coloring 2D map...\n") + progress_bar = tqdm.tqdm(total = len(change_mark)) + self.pcd_map_2d = pd.DataFrame({"x" : np.zeros(self.point_num), + "y" : np.zeros(self.point_num), + "color" : ["" for j in range(self.point_num)]}) + + print("Length of change mark = {0}".format(len(change_mark))) + i = 0 + + for idx, value in enumerate(change_mark): + progress_bar.update(1) + if value == 0: + color = "blue" + else: + color = "red" + # Insert points with color to the 2D map + for p in self.segment_dict["segment_df"].loc[idx, "pcd"]: + self.pcd_map_2d.loc[i] = [p[0], p[1], color] + i += 1 + + # Downsample a point cloud + def __downsample(self, cloud: np.array) -> np.array: + return cloud + + # Save the tp and average tps to CSV file ##### + def save_results(self, result_path: str): + self.ndt_res_df.to_csv(result_path) + + # Plot the points with color from the 2D segment map + def show(self, result_path: str): + print("Plotting") + plt.scatter(self.pcd_map_2d["x"], self.pcd_map_2d["y"], + c = self.pcd_map_2d["color"]) + + plt.legend() + plt.grid() + plt.savefig(result_path) + + try: + plt.show() + except KeyboardInterrupt: + print("Quit by keyboard interrupt") + + def check(self, rosbag_path: str): + # Read the rosbag and get the ndt poses and corresponding tps + self.__collect_rosbag_tp(rosbag_path) + # Update the TPs of segments + self.__mark_changes() + # Save the new TPs + self.save_results() + # Display the result + self.show() if __name__ == "__main__": parser = argparse.ArgumentParser() @@ -198,4 +279,7 @@ def processing(pcd_map_dir: str, rosbag_path: str): print("Input rosbag at %s" % (args.bag_path)) # Run - processing(args.map_path, args.bag_path) + checker = TPChecker() + + checker.set_pcd_map(args.map_path) + checker.check(args.bag_path) diff --git a/map/autoware_tp_collector/scripts/tp_collector.py b/map/autoware_tp_collector/scripts/tp_collector.py index f8629d2d..6f0558eb 100755 --- a/map/autoware_tp_collector/scripts/tp_collector.py +++ b/map/autoware_tp_collector/scripts/tp_collector.py @@ -54,15 +54,6 @@ def compute_total_message_count(rosbag_path): return total_message_count -##### Convert names of pcd segments to 2d coordinates ##### -def name_to_coordinate(seg_name: str): - # Remove the extension - name_only = os.path.splitext(seg_name)[0] - str_coors = name_only.split("_") - - # Return the coordinates as a 1x2 ndarray - return np.array(str_coors, dtype = float).reshape(1, 2) - ##### Read the YAML file to get the list of PCD segments and scores ##### def get_pcd_segments_and_scores(pcd_map_dir: str) -> dict: if not os.path.exists(pcd_map_dir): From eaad31604667971e07b3f1c776a334c3027c9fa4 Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Mon, 27 Jan 2025 22:05:58 +0700 Subject: [PATCH 5/7] Publish results of checking to Rviz2 Signed-off-by: Anh Nguyen --- .../scripts/tp_checker.py | 276 +++++++----- .../scripts/tp_collector.py | 415 +++++++++++------- 2 files changed, 417 insertions(+), 274 deletions(-) diff --git a/map/autoware_tp_collector/scripts/tp_checker.py b/map/autoware_tp_collector/scripts/tp_checker.py index 8831f418..56198c11 100755 --- a/map/autoware_tp_collector/scripts/tp_checker.py +++ b/map/autoware_tp_collector/scripts/tp_checker.py @@ -33,48 +33,40 @@ from scipy import spatial as sp import numpy as np from geometry_msgs.msg import PoseWithCovarianceStamped +import sensor_msgs.msg as sensor_msgs +import std_msgs.msg as std_msgs +import rclpy +from rclpy.node import Node from tier4_debug_msgs.msg import Float32Stamped from builtin_interfaces.msg import Time import pandas as pd -import open3d as o3d -import matplotlib.pyplot as plt import tqdm +import time +import shutil +import open3d as o3d -def compute_total_message_count(rosbag_path): - if rosbag_path.endswith(".db3"): - yaml_path = os.path.join(os.path.dirname(rosbag_path), "metadata.yaml") - else: - yaml_path = os.path.join(rosbag_path, "metadata.yaml") - - with open(yaml_path, "r") as f: - data = yaml.safe_load(f) - - total_message_count = 0 - for topic_data in data["rosbag2_bagfile_information"]["topics_with_message_count"]: - total_message_count += topic_data["message_count"] - - return total_message_count +class TPChecker(Node): -class TPChecker: def __init__(self): - self.segment_dict = {} # Segment indices, TP, kdtree - self.base_path = "" # Path to the base directory of the PCD segments - self.ndt_res_df = pd.DataFrame() # A set of tp read from a rosbag - self.pcd_map_2d = pd.DataFrame(columns = ["x", "y", "color"]) - # A 2D map of pcd, color indicates the map changes - self.point_num = 0 + super().__init__('tp_checker') + self.segment_df = None # Segment indices, TP, kdtree + self.kdtree = None # A 2D kdtree to search for segments in the vicinity of poses + self.ndt_res_df = None # A set of tp read from a rosbag + 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 # Read the input map directory and setup the segment dictionary - def set_pcd_map(self, pcd_map_dir: str): + def __set_pcd_map(self, pcd_map_dir: str): # Check if the required folders/files exist if not os.path.exists(pcd_map_dir): print("Error: {0} does not exist!".format(pcd_map_dir)) exit() - pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd") + self.pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd") - if not os.path.exists(pcd_path): - print("Error: {0} does not exist!".format(pcd_path)) + if not os.path.exists(self.pcd_path): + print("Error: {0} does not exist!".format(self.pcd_path)) exit() metadata_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") @@ -83,45 +75,37 @@ def set_pcd_map(self, pcd_map_dir: str): print("Error: {0} does not exist!".format(metadata_path)) exit() - score_path = os.path.join(pcd_map_dir, "score.csv") + score_path = os.path.join(pcd_map_dir, "scores.csv") if not os.path.exists(score_path): print("Error: {0} does not exist!".format(score_path)) exit() # Read the metadata file and get the list of segments - segment_df = pd.DataFrame(columns = ["x", "y", "tp", "pcd"]) + print("Read the PCD map at {0}".format(pcd_map_dir)) + self.segment_df = pd.DataFrame(columns = ["x", "y", "tp", "seg_key"]) with open(metadata_path, "r") as f: for key, value in yaml.safe_load(f).items(): if key != "x_resolution" and key != "y_resolution": - # Read PCD files and store them into a 2D map - seg_path = os.path.join(pcd_path, key) - pcd = o3d.io.read_point_cloud(seg_path) - # Downsample the segment to a greater resolution - # This segment is just for visualizing the result, no need to be detailed - ds_pcd = self.__downsample(np.asarray(pcd.points)) - segment_df.loc[len(segment_df)] = [float(value[0]), float(value[1]), 0, ds_pcd] - self.point_num += len(ds_pcd) + self.segment_df.loc[len(self.segment_df)] = [float(value[0]), float(value[1]), 0, key] + # Create a 2D kdtree on the segments - seg_tree_nodes = np.zeros((len(segment_df), 2), dtype = float) + seg_tree_nodes = np.zeros((len(self.segment_df), 2), dtype = float) - for index, row in segment_df.iterrows(): + for index, row in self.segment_df.iterrows(): seg_tree_nodes[index, :] = [row["x"], row["y"]] - kdtree = sp.KDTree(seg_tree_nodes) + self.kdtree = sp.KDTree(seg_tree_nodes) # Read the score file with open(score_path, "r") as f: reader = csv.reader(f) for index, row in enumerate(reader): - segment_df.loc[index, "tp"] = float(row[1]) + self.segment_df.loc[index, "tp"] = float(row[1]) - self.segment_dict["segment_df"] = segment_df - self.segment_dict["kdtree"] = kdtree - self.base_path = pcd_path ##### Stamp search ##### def __stamp_search(self, stamp: int, tp_df: pd.DataFrame) -> int: @@ -151,8 +135,10 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: ) reader.open(bag_storage_options, bag_converter_options) - total_message_count = compute_total_message_count(bag_path) - progress_bar = tqdm.tqdm(total=total_message_count) + total_message_count = self.__compute_total_message_count(bag_path) + + print("Read the input rosbag at {0}".format(bag_path)) + progress_bar = tqdm.tqdm(total = total_message_count) pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) tp_df = pd.DataFrame(columns = ["stamp", "tp"]) @@ -171,11 +157,14 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: stamp = tp_msg.stamp.sec * 1e9 + tp_msg.stamp.nanosec tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] - + + progress_bar.close() # Now from the two list above build a table of estimated pose and corresponding TPs - self.ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp", "avg_tp"]) + self.ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp", "expected_tp"]) - progress_bar = tqdm.tqdm(total=len(pose_df)) + print("Prepare the tp list...") + + progress_bar = tqdm.tqdm(total = len(pose_df)) for row in pose_df.itertuples(): progress_bar.update(1) @@ -186,18 +175,21 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: tid = self.__stamp_search(stamp, tp_df) if tid >= 0: - self.ndt_res_df[len(self.ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid], 0.0] + self.ndt_res_df.loc[len(self.ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid], 0.0] + + progress_bar.close() + # Mark the segments which may has changed def __mark_changes(self): - print("Checking map changes...\n") - change_mark = np.zeros(len(self.segment_dict["segment_df"]), dtype = int) + print("Checking map changes... tp len = {0}".format(len(self.ndt_res_df))) + self.change_mark = np.zeros(len(self.segment_df), dtype = int) progress_bar = tqdm.tqdm(total = len(self.ndt_res_df)) - for row in self.ndt_res_df.itertuples(): + for i, row in self.ndt_res_df.iterrows(): progress_bar.update(1) - nn_idx = self.segment_dict["kdtree"].query_ball_point([row.x, row.y], 20.0) + nn_idx = self.kdtree.query_ball_point([row.x, row.y], 20.0) if len(nn_idx) == 0: continue @@ -205,81 +197,153 @@ def __mark_changes(self): sum_tp = 0.0 for idx in nn_idx: - sum_tp += self.segment_dict["segment_df"].loc[idx, "tp"] + sum_tp += self.segment_df.loc[idx, "tp"] - row.avg_tp = sum_tp / len(nn_idx) + self.ndt_res_df.loc[i, "expected_tp"] = sum_tp / float(len(nn_idx)) - if abs(row.tp - row.avg_tp) / row.tp >= 0.2: + if abs(row.tp - row.expected_tp) / row.tp >= 0.2: for idx in nn_idx: - change_mark[idx] = 1 - - # Export the 2D map - print("Coloring 2D map...\n") - progress_bar = tqdm.tqdm(total = len(change_mark)) - self.pcd_map_2d = pd.DataFrame({"x" : np.zeros(self.point_num), - "y" : np.zeros(self.point_num), - "color" : ["" for j in range(self.point_num)]}) - - print("Length of change mark = {0}".format(len(change_mark))) - i = 0 + self.change_mark[idx] = 1 + + progress_bar.close() - for idx, value in enumerate(change_mark): + # Save the changed segments + progress_bar = tqdm.tqdm(total = len(self.change_mark)) + + for idx, value in enumerate(self.change_mark): progress_bar.update(1) - if value == 0: - color = "blue" - else: - color = "red" - # Insert points with color to the 2D map - for p in self.segment_dict["segment_df"].loc[idx, "pcd"]: - self.pcd_map_2d.loc[i] = [p[0], p[1], color] - i += 1 - - # Downsample a point cloud - def __downsample(self, cloud: np.array) -> np.array: - return cloud + # Copy changed segments to the destinated folder + seg_key = self.segment_df.loc[idx, "seg_key"] + + if value != 0: + shutil.copyfile(os.path.join(self.pcd_path, seg_key), + os.path.join(self.changed_dir, seg_key)) + progress_bar.close() + + + def __compute_total_message_count(self, rosbag_path): + if rosbag_path.endswith(".db3"): + yaml_path = os.path.join(os.path.dirname(rosbag_path), "metadata.yaml") + else: + yaml_path = os.path.join(rosbag_path, "metadata.yaml") + + with open(yaml_path, "r") as f: + data = yaml.safe_load(f) + + total_message_count = 0 + for topic_data in data["rosbag2_bagfile_information"]["topics_with_message_count"]: + total_message_count += topic_data["message_count"] + + return total_message_count + # Save the tp and average tps to CSV file ##### - def save_results(self, result_path: str): - self.ndt_res_df.to_csv(result_path) - - # Plot the points with color from the 2D segment map - def show(self, result_path: str): - print("Plotting") - plt.scatter(self.pcd_map_2d["x"], self.pcd_map_2d["y"], - c = self.pcd_map_2d["color"]) - - plt.legend() - plt.grid() - plt.savefig(result_path) + def __save_results(self: str): + self.ndt_res_df.to_csv(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)] + + points = [] + pc2_width = 0 + + progress_bar = tqdm.tqdm(total = len(self.segment_df)) + origin = None + + for i, tuple in self.segment_df.iterrows(): + progress_bar.update(1) + # Load the current segment + pcd = o3d.io.read_point_cloud(os.path.join(self.pcd_path, tuple.seg_key)) + np_pcd = np.asarray(pcd.points) + rgba = self.__set_color_based_on_mark(self.change_mark[i]) + + for p in np_pcd: + if origin == None: + origin = [p[0], p[1], p[2]] + pt = [p[0] - origin[0], p[1] - origin[1], p[2] - origin[2], rgba] + points.append(pt) + pc2_width += 1 + + header = std_msgs.Header() + header.frame_id = "map" + pc2_msg = sensor_msgs.PointCloud2( + header = header, + height = 1, + width = pc2_width, + is_dense = False, + is_bigendian = False, + fields = fields, + point_step = itemsize * 4, + row_step = itemsize * 4 * pc2_width, + data = np.array(points).astype(dtype).tobytes() + ) + + 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 == 1: + r = 255 + g = 0 + b = 0 + else: + r = 255 + g = 255 + b = 255 + + rgb = (r << 16) | (g << 8) | b + + return rgb + + def processing(self, pcd_path: str, rosbag_path: str, result_path: str): + if not os.path.exists(result_path): + os.makedirs(result_path) + else: + shutil.rmtree(result_path) + os.makedirs(result_path) + + self.changed_dir = os.path.join(result_path, "changed_dir") + os.makedirs(self.changed_dir) + self.result_csv = os.path.join(result_path, "result.csv") - try: - plt.show() - except KeyboardInterrupt: - print("Quit by keyboard interrupt") + self.__set_pcd_map(pcd_path) - def check(self, rosbag_path: str): # Read the rosbag and get the ndt poses and corresponding tps self.__collect_rosbag_tp(rosbag_path) # Update the TPs of segments self.__mark_changes() - # Save the new TPs - self.save_results() - # Display the result - self.show() + 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") args = parser.parse_args() # Practice with string % a bit - print("Input PCD map at %s" % (args.map_path)) - print("Input rosbag at %s" % (args.bag_path)) + print("Input PCD map at {0}".format(args.map_path)) + print("Input rosbag at {0}".format(args.bag_path)) + print("Results are saved at {0}".format(args.result_path)) # Run checker = TPChecker() - checker.set_pcd_map(args.map_path) - checker.check(args.bag_path) + checker.processing(args.map_path, args.bag_path, args.result_path) diff --git a/map/autoware_tp_collector/scripts/tp_collector.py b/map/autoware_tp_collector/scripts/tp_collector.py index 6f0558eb..22ef206c 100755 --- a/map/autoware_tp_collector/scripts/tp_collector.py +++ b/map/autoware_tp_collector/scripts/tp_collector.py @@ -37,177 +37,255 @@ from builtin_interfaces.msg import Time import pandas as pd import tqdm - - -def compute_total_message_count(rosbag_path): - if rosbag_path.endswith(".db3"): - yaml_path = os.path.join(os.path.dirname(rosbag_path), "metadata.yaml") - else: - yaml_path = os.path.join(rosbag_path, "metadata.yaml") - - with open(yaml_path, "r") as f: - data = yaml.safe_load(f) - - total_message_count = 0 - for topic_data in data["rosbag2_bagfile_information"]["topics_with_message_count"]: - total_message_count += topic_data["message_count"] - - return total_message_count - -##### Read the YAML file to get the list of PCD segments and scores ##### -def get_pcd_segments_and_scores(pcd_map_dir: str) -> dict: - if not os.path.exists(pcd_map_dir): - print("Error: %s does not exist!"%(pcd_map_dir)) - exit() - - pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd/") - - if not os.path.exists(pcd_path): - print("Error: %s does not exist!"%(pcd_path)) - exit() - - yaml_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") - - # Create a dataframe to record the avg tp of 2D segments - segment_df = pd.DataFrame(columns = ["x", "y", "tp"]) - - with open(yaml_path, "r") as f: - for key, value in yaml.safe_load(f).items(): - if key != "x_resolution" and key != "y_resolution": - segment_df.loc[len(segment_df)] = [float(value[0]), float(value[1]), 0] - - # A 2D array that contains the 2D coordinates of segments - # We'll use this to build a KDtree - seg_tree_nodes = np.zeros((len(segment_df), 2), dtype = float) - - for index, row in segment_df.iterrows(): - seg_tree_nodes[index,:] = [row["x"], row["y"]] - - # Create a 2D kdtree on the segment list - kdtree = sp.KDTree(seg_tree_nodes) - - # Load the score map - score_path = os.path.join(pcd_map_dir, "scores.csv") - - if os.path.exists(score_path): - with open(score_path, "r") as f: - reader = csv.reader(f) - for index, row in enumerate(reader): - segment_df.loc[index, "tp"] = float(row[1]) - else: - # If the score file does not exist, initialize scores to all 0 - segment_df.loc[:,"tp"] = 0 - - return {"segment_df": segment_df, "kdtree": kdtree} - -##### Stamp search ##### -def stamp_search(stamp: int, tp_df: pd.DataFrame) -> int: - left = 0 - right = len(tp_df) - res = -1 - - # Find the closest stamp that goes before the stamp key - while left < right: - mid = (left + right) // 2 - cur_stamp = tp_df["stamp"][mid] - - if cur_stamp <= stamp: - left = mid + 1 - res = mid - elif cur_stamp > stamp: - right = mid - 1 - - return res - -##### Read the input rosbag to obtain ndt pose and TP values ##### -def collect_rosbag_tp(bag_path: str) -> pd.DataFrame: - reader = SequentialReader() - bag_storage_options = StorageOptions(uri = bag_path, storage_id = "sqlite3") - bag_converter_options = ConverterOptions( - input_serialization_format = "cdr", output_serialization_format = "cdr" - ) - reader.open(bag_storage_options, bag_converter_options) - - total_message_count = compute_total_message_count(bag_path) - progress_bar = tqdm.tqdm(total=total_message_count) - - - pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) - tp_df = pd.DataFrame(columns = ["stamp", "tp"]) - - while reader.has_next(): - progress_bar.update(1) - (topic, data, stamp) = reader.read_next() - - if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": - pose_msg = deserialize_message(data, PoseWithCovarianceStamped) - stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec - pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] - elif topic == "/localization/pose_estimator/transform_probability": - tp_msg = deserialize_message(data, Float32Stamped) - stamp = tp_msg.stamp.sec * 1e9 + tp_msg.stamp.nanosec - - tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] - - # Now from the two list above build a table of estimated pose and corresponding TPs - ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp"]) - - progress_bar = tqdm.tqdm(total=len(pose_df)) - - for row in pose_df.itertuples(): - progress_bar.update(1) - stamp = row.stamp - pose = row.pose_msg - # Find the tp whose stamp is the closest to the pose - tid = stamp_search(stamp, tp_df) +import open3d as o3d +import sensor_msgs.msg as sensor_msgs +import std_msgs.msg as std_msgs +import rclpy +from rclpy.node import Node +import time + +class TPCollector(Node): + def __init__(self): + super().__init__('tp_collector') + self.pcd_path = None + self.yaml_path = None + self.score_path = None + self.segment_df = None + self.kdtree = None + + def __compute_total_message_count(self, rosbag_path): + if rosbag_path.endswith(".db3"): + yaml_path = os.path.join(os.path.dirname(rosbag_path), "metadata.yaml") + else: + yaml_path = os.path.join(rosbag_path, "metadata.yaml") + + with open(yaml_path, "r") as f: + data = yaml.safe_load(f) + + total_message_count = 0 + for topic_data in data["rosbag2_bagfile_information"]["topics_with_message_count"]: + total_message_count += topic_data["message_count"] + + return total_message_count + + ##### Read the YAML file to get the list of PCD segments and scores ##### + def __get_pcd_segments_and_scores(self, pcd_map_dir: str): + if not os.path.exists(pcd_map_dir): + print("Error: %s does not exist!"%(pcd_map_dir)) + exit() + + self.pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd/") + + if not os.path.exists(self.pcd_path): + print("Error: %s does not exist!"%(self.pcd_path)) + exit() + + self.yaml_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") + + # Create a dataframe to record the avg tp of 2D segments + self.segment_df = pd.DataFrame(columns = ["x", "y", "tp", "key"]) + + with open(self.yaml_path, "r") as f: + for key, value in yaml.safe_load(f).items(): + if key != "x_resolution" and key != "y_resolution": + self.segment_df.loc[len(self.segment_df)] = [float(value[0]), float(value[1]), 0, key] + + # A 2D array that contains the 2D coordinates of segments + # We'll use this to build a KDtree + seg_tree_nodes = np.zeros((len(self.segment_df), 2), dtype = float) + + for index, row in self.segment_df.iterrows(): + seg_tree_nodes[index,:] = [row["x"], row["y"]] + + # Create a 2D kdtree on the segment list + self.kdtree = sp.KDTree(seg_tree_nodes) + + # Load the score map + self.score_path = os.path.join(pcd_map_dir, "scores.csv") + + if os.path.exists(self.score_path): + with open(self.score_path, "r") as f: + reader = csv.reader(f) + for index, row in enumerate(reader): + self.segment_df.loc[index, "tp"] = float(row[1]) + else: + # If the score file does not exist, initialize scores to all 0 + self.segment_df.loc[:,"tp"] = 0 - if tid >= 0: - ndt_res_df.loc[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid]] + ##### Stamp search ##### + def __stamp_search(self, stamp: int, tp_df: pd.DataFrame) -> int: + left = 0 + right = len(tp_df) + res = -1 + + # Find the closest stamp that goes before the stamp key + while left < right: + mid = (left + right) // 2 + cur_stamp = tp_df["stamp"][mid] + + if cur_stamp <= stamp: + left = mid + 1 + res = mid + elif cur_stamp > stamp: + right = mid - 1 + + return res + + ##### Read the input rosbag to obtain ndt pose and TP values ##### + def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: + reader = SequentialReader() + bag_storage_options = StorageOptions(uri = bag_path, storage_id = "sqlite3") + bag_converter_options = ConverterOptions( + input_serialization_format = "cdr", output_serialization_format = "cdr" + ) + reader.open(bag_storage_options, bag_converter_options) + + total_message_count = self.__compute_total_message_count(bag_path) + progress_bar = tqdm.tqdm(total=total_message_count) + + + pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) + tp_df = pd.DataFrame(columns = ["stamp", "tp"]) + + while reader.has_next(): + progress_bar.update(1) + (topic, data, stamp) = reader.read_next() + + if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": + pose_msg = deserialize_message(data, PoseWithCovarianceStamped) + stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec + pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] + elif topic == "/localization/pose_estimator/transform_probability": + tp_msg = deserialize_message(data, Float32Stamped) + stamp = tp_msg.stamp.sec * 1e9 + tp_msg.stamp.nanosec + + tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] - return ndt_res_df - -##### Update map's TP ##### -def update_avg_tp(ndt_res_df: pd.DataFrame, segment_dict: dict): - print("UPDATE AVG TP, len of ndt_res_df = {0}".format(len(ndt_res_df))) - # Iterate on poses and the corresponding TPs - progress_bar = tqdm.tqdm(total=len(ndt_res_df)) - - for tp_row in ndt_res_df.itertuples(): - # Find indices of map segments that cover the current pose - nn_idx = segment_dict["kdtree"].query_ball_point([tp_row.x, tp_row.y], 20.0) - - for idx in nn_idx: - # If the TP from rosbag is greater than the current TP of the segment, - # replace the segment TP with the TP from the rosbag - # TODO: record the average TP of segments - if tp_row.tp > segment_dict["segment_df"].loc[idx, "tp"]: - segment_dict["segment_df"].loc[idx, "tp"] = tp_row.tp - -##### Save the segment TPs ##### -def save_tps(pcd_map_dir: str, segment_dict: dict): - print("SAVE AVG TP") - - score_path = os.path.join(pcd_map_dir, "score.csv") - - print(segment_dict) - - with open(score_path, "w") as f: - for row in segment_dict["segment_df"].itertuples(): - f.write("{0}_{1},{2}\n".format(int(row.x), int(row.y), row.tp)) - -def processing(pcd_map_dir: str, rosbag_path: str): - # Get the segment lists and scores - segment_dict = get_pcd_segments_and_scores(pcd_map_dir) - print("PAUSE at 179 segment dict is \n{0}".format(segment_dict)) - - # Read the rosbag and get the ndt poses and corresponding tps - ndt_res_df = collect_rosbag_tp(rosbag_path) - # Update the TPs of segments - update_avg_tp(ndt_res_df, segment_dict) - # Save the new TPs - save_tps(pcd_map_dir, segment_dict) + # Now from the two list above build a table of estimated pose and corresponding TPs + ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp"]) + + progress_bar = tqdm.tqdm(total=len(pose_df)) + + for row in pose_df.itertuples(): + progress_bar.update(1) + stamp = row.stamp + pose = row.pose_msg + # Find the tp whose stamp is the closest to the pose + tid = self.__stamp_search(stamp, tp_df) + + if tid >= 0: + ndt_res_df.loc[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid]] + + return ndt_res_df + + ##### Update map's TP ##### + def __update_avg_tp(self, ndt_res_df: pd.DataFrame): + print("UPDATE AVG TP, len of ndt_res_df = {0}".format(len(ndt_res_df))) + # Iterate on poses and the corresponding TPs + progress_bar = tqdm.tqdm(total=len(ndt_res_df)) + + for tp_row in ndt_res_df.itertuples(): + # Find indices of map segments that cover the current pose + nn_idx = self.kdtree.query_ball_point([tp_row.x, tp_row.y], 20.0) + + for idx in nn_idx: + # If the TP from rosbag is greater than the current TP of the segment, + # replace the segment TP with the TP from the rosbag + # TODO: record the average TP of segments + if tp_row.tp > self.segment_df.loc[idx, "tp"]: + self.segment_df.loc[idx, "tp"] = tp_row.tp + + ##### Save the segment TPs ##### + def __save_tps(self): + with open(self.score_path, "w") as f: + for row in self.segment_df.itertuples(): + f.write("{0}_{1},{2}\n".format(int(row.x), int(row.y), row.tp)) + + 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)] + + points = [] + pc2_width = 0 + + progress_bar = tqdm.tqdm(total = len(self.segment_df)) + origin = None + + for tuple in self.segment_df.itertuples(): + progress_bar.update(1) + # Load the current segment + pcd = o3d.io.read_point_cloud(os.path.join(self.pcd_path, tuple.key)) + np_pcd = np.asarray(pcd.points) + rgba = self.__set_color_based_on_score(tuple.tp) + + for p in np_pcd: + if origin == None: + origin = [p[0], p[1], p[2]] + pt = [p[0] - origin[0], p[1] - origin[1], p[2] - origin[2], rgba] + points.append(pt) + pc2_width += 1 + + header = std_msgs.Header() + header.frame_id = "map" + pc2_msg = sensor_msgs.PointCloud2( + header = header, + height = 1, + width = pc2_width, + is_dense = False, + is_bigendian = False, + fields = fields, + point_step = itemsize * 4, + row_step = itemsize * 4 * pc2_width, + data = np.array(points).astype(dtype).tobytes() + ) + + pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, '/autoware_tp_collector', 10) + + while True: + pcd_publisher.publish(pc2_msg) + time.sleep(5) + + + def __set_color_based_on_score(self, score) -> int: + if score < 3.0: + r = 255 + g = 0 + b = 0 + else: + r = 255 + g = 255 + b = 255 + + # rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] + rgb = (r << 16) | (g << 8) | b + + return rgb + + + def processing(self, pcd_map_dir: str, rosbag_path: str): + # Get the segment lists and scores + self.__get_pcd_segments_and_scores(pcd_map_dir) + + # Read the rosbag and get the ndt poses and corresponding tps + ndt_res_df = self.__collect_rosbag_tp(rosbag_path) + + # Update the TPs of segments + self.__update_avg_tp(ndt_res_df) + + # Save the new TPs + self.__save_tps() + + 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") @@ -219,4 +297,5 @@ def processing(pcd_map_dir: str, rosbag_path: str): print("Input rosbag at %s" % (args.bag_path)) # Run - processing(args.map_path, args.bag_path) + tp_collector = TPCollector() + tp_collector.processing(args.map_path, args.bag_path) From 40a99114aa35cc65d974f8c2453ea2fb0937ebfa Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Thu, 6 Feb 2025 10:24:58 +0700 Subject: [PATCH 6/7] Add a tool to visualize segment scores on rviz Signed-off-by: Anh Nguyen --- map/autoware_tp_collector/CMakeLists.txt | 10 +- .../scripts/tp_checker.py | 29 ++-- .../scripts/tp_collector.py | 91 ++-------- .../scripts/tp_visualizer.py | 156 ++++++++++++++++++ 4 files changed, 190 insertions(+), 96 deletions(-) create mode 100644 map/autoware_tp_collector/scripts/tp_visualizer.py diff --git a/map/autoware_tp_collector/CMakeLists.txt b/map/autoware_tp_collector/CMakeLists.txt index 773e0f9b..0a05c7c6 100644 --- a/map/autoware_tp_collector/CMakeLists.txt +++ b/map/autoware_tp_collector/CMakeLists.txt @@ -15,10 +15,12 @@ endif () find_package(yaml-cpp REQUIRED) find_package(PCL REQUIRED) -install(PROGRAMS - scripts/tp_collector.py - scripts/tp_checker.py +install(PROGRAMS + scripts/tp_collector.py + scripts/tp_checker.py + scripts/tp_visualizer.py DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package(INSTALL_TO_SHARE launch config) +# ament_auto_package(INSTALL_TO_SHARE launch config) +ament_auto_package() diff --git a/map/autoware_tp_collector/scripts/tp_checker.py b/map/autoware_tp_collector/scripts/tp_checker.py index 56198c11..43642481 100755 --- a/map/autoware_tp_collector/scripts/tp_checker.py +++ b/map/autoware_tp_collector/scripts/tp_checker.py @@ -19,11 +19,7 @@ from rosbag2_py import ( SequentialReader, StorageOptions, - ConverterOptions, - SequentialWriter, - BagMetadata, - TopicMetadata, - Info + ConverterOptions ) from rclpy.serialization import deserialize_message, serialize_message @@ -44,6 +40,8 @@ import time import shutil import open3d as o3d +import sensor_msgs_py.point_cloud2 as pc2 +import struct class TPChecker(Node): @@ -240,6 +238,7 @@ def __compute_total_message_count(self, rosbag_path): # Save the tp and average tps to CSV file ##### def __save_results(self: str): 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 @@ -272,20 +271,12 @@ def __show(self): points.append(pt) pc2_width += 1 + print("Publishing result...") header = std_msgs.Header() + header.stamp = self.get_clock().now().to_msg() header.frame_id = "map" - pc2_msg = sensor_msgs.PointCloud2( - header = header, - height = 1, - width = pc2_width, - is_dense = False, - is_bigendian = False, - fields = fields, - point_step = itemsize * 4, - row_step = itemsize * 4 * pc2_width, - data = np.array(points).astype(dtype).tobytes() - ) + pc2_msg = pc2.create_cloud(header, fields, points) pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, '/autoware_tp_checker', 10) while True: @@ -304,10 +295,12 @@ def __set_color_based_on_mark(self, mark) -> int: r = 255 g = 255 b = 255 + a = 255 - rgb = (r << 16) | (g << 8) | b + tmp_rgb = struct.pack('BBBB', b, g, r, a) + rgba = struct.unpack('I', tmp_rgb)[0] - return rgb + return rgba def processing(self, pcd_path: str, rosbag_path: str, result_path: str): if not os.path.exists(result_path): diff --git a/map/autoware_tp_collector/scripts/tp_collector.py b/map/autoware_tp_collector/scripts/tp_collector.py index 22ef206c..3bffe207 100755 --- a/map/autoware_tp_collector/scripts/tp_collector.py +++ b/map/autoware_tp_collector/scripts/tp_collector.py @@ -40,6 +40,7 @@ import open3d as o3d import sensor_msgs.msg as sensor_msgs import std_msgs.msg as std_msgs +import sensor_msgs_py.point_cloud2 as pc2 import rclpy from rclpy.node import Node import time @@ -140,6 +141,8 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: ) reader.open(bag_storage_options, bag_converter_options) + print("Reading rosbag...") + total_message_count = self.__compute_total_message_count(bag_path) progress_bar = tqdm.tqdm(total=total_message_count) @@ -151,7 +154,8 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: progress_bar.update(1) (topic, data, stamp) = reader.read_next() - if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": + # if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": + if topic == "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias": pose_msg = deserialize_message(data, PoseWithCovarianceStamped) stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] @@ -161,6 +165,8 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] + progress_bar.close() + # Now from the two list above build a table of estimated pose and corresponding TPs ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp"]) @@ -175,16 +181,19 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: if tid >= 0: ndt_res_df.loc[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid]] - + + progress_bar.close() + return ndt_res_df ##### Update map's TP ##### def __update_avg_tp(self, ndt_res_df: pd.DataFrame): - print("UPDATE AVG TP, len of ndt_res_df = {0}".format(len(ndt_res_df))) + print("Updating segments' TP...") # Iterate on poses and the corresponding TPs progress_bar = tqdm.tqdm(total=len(ndt_res_df)) for tp_row in ndt_res_df.itertuples(): + progress_bar.update(1) # Find indices of map segments that cover the current pose nn_idx = self.kdtree.query_ball_point([tp_row.x, tp_row.y], 20.0) @@ -194,80 +203,16 @@ def __update_avg_tp(self, ndt_res_df: pd.DataFrame): # TODO: record the average TP of segments if tp_row.tp > self.segment_df.loc[idx, "tp"]: self.segment_df.loc[idx, "tp"] = tp_row.tp + progress_bar.close() ##### 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") for row in self.segment_df.itertuples(): - f.write("{0}_{1},{2}\n".format(int(row.x), int(row.y), row.tp)) - - 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)] - - points = [] - pc2_width = 0 - - progress_bar = tqdm.tqdm(total = len(self.segment_df)) - origin = None - - for tuple in self.segment_df.itertuples(): - progress_bar.update(1) - # Load the current segment - pcd = o3d.io.read_point_cloud(os.path.join(self.pcd_path, tuple.key)) - np_pcd = np.asarray(pcd.points) - rgba = self.__set_color_based_on_score(tuple.tp) - - for p in np_pcd: - if origin == None: - origin = [p[0], p[1], p[2]] - pt = [p[0] - origin[0], p[1] - origin[1], p[2] - origin[2], rgba] - points.append(pt) - pc2_width += 1 - - header = std_msgs.Header() - header.frame_id = "map" - pc2_msg = sensor_msgs.PointCloud2( - header = header, - height = 1, - width = pc2_width, - is_dense = False, - is_bigendian = False, - fields = fields, - point_step = itemsize * 4, - row_step = itemsize * 4 * pc2_width, - data = np.array(points).astype(dtype).tobytes() - ) - - pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, '/autoware_tp_collector', 10) - - while True: - pcd_publisher.publish(pc2_msg) - time.sleep(5) - - - def __set_color_based_on_score(self, score) -> int: - if score < 3.0: - r = 255 - g = 0 - b = 0 - else: - r = 255 - g = 255 - b = 255 - - # rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] - rgb = (r << 16) | (g << 8) | b - - return rgb - + f.write("{0},{1}\n".format(row.key, row.tp)) + print("Done. Segments' TPs are saved at {0}.".format(self.score_path)) def processing(self, pcd_map_dir: str, rosbag_path: str): # Get the segment lists and scores @@ -282,8 +227,6 @@ def processing(self, pcd_map_dir: str, rosbag_path: str): # Save the new TPs self.__save_tps() - self.__show() - if __name__ == "__main__": rclpy.init() parser = argparse.ArgumentParser() diff --git a/map/autoware_tp_collector/scripts/tp_visualizer.py b/map/autoware_tp_collector/scripts/tp_visualizer.py new file mode 100644 index 00000000..56846fae --- /dev/null +++ b/map/autoware_tp_collector/scripts/tp_visualizer.py @@ -0,0 +1,156 @@ +#!/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. + +import os +import argparse +from rosbag2_py import ( + SequentialReader, + StorageOptions, + ConverterOptions, + SequentialWriter, + BagMetadata, + TopicMetadata, + 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 +from geometry_msgs.msg import PoseWithCovarianceStamped +from tier4_debug_msgs.msg import Float32Stamped +from builtin_interfaces.msg import Time +import pandas as pd +import tqdm +import open3d as o3d +import sensor_msgs.msg as sensor_msgs +import std_msgs.msg as std_msgs +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') + self.pcd_path = None + self.yaml_path = None + self.score_path = None + + ##### Read the YAML file to get the list of PCD segments and scores ##### + def __get_pcd_segments_and_scores(self, pcd_map_dir: str): + if not os.path.exists(pcd_map_dir): + print("Error: the input PCD folder does not exist at {0}! Abort!".format(pcd_map_dir)) + exit() + + self.pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd/") + + if not os.path.exists(self.pcd_path): + print("Error: no PCD file was found at {0}! Abort!".format(self.pcd_path)) + exit() + + self.score_path = os.path.join(pcd_map_dir, "scores.csv") + + if not os.path.exists(self.score_path): + print("Error: a score file does not exist at {0}".format(self.score_path)) + exit() + + self.segment_df = pd.read_csv(self.score_path) + + 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)] + + points = [] + pc2_width = 0 + + progress_bar = tqdm.tqdm(total = len(self.segment_df)) + origin = None + + for tuple in self.segment_df.itertuples(): + progress_bar.update(1) + # Load the current segment + seg_path = self.pcd_path + "/" + tuple.segment + pcd = o3d.io.read_point_cloud(seg_path) + np_pcd = np.asarray(pcd.points) + rgba = self.__set_color_based_on_score(tuple.tp) + + for p in np_pcd: + if origin == None: + origin = [p[0], p[1], p[2]] + pt = [p[0] - origin[0], p[1] - origin[1], p[2] - origin[2], rgba] + points.append(pt) + pc2_width += 1 + + progress_bar.close() + + print("Publishing result...") + header = std_msgs.Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = "map" + + pc2_msg = pc2.create_cloud(header, fields, points) + 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: + color_code = int(score / 7.0 * 10.0) + step = 25 + + r = 255 - (step * color_code) + g = 255 + b = 255 + a = 255 + + tmp_rgb = struct.pack('BBBB', b, g, r, a) + rgba = struct.unpack('I', tmp_rgb)[0] + + return rgba + + + def processing(self, pcd_map_dir: str): + # Get the segment lists and scores + self.__get_pcd_segments_and_scores(pcd_map_dir) + # Publish to rviz + self.__show() + +if __name__ == "__main__": + rclpy.init() + parser = argparse.ArgumentParser() + parser.add_argument("map_path", help="The path to the PCD folder") + + args = parser.parse_args() + + # Practice with string % a bit + print("Input PCD map at %s" % (args.map_path)) + + # Run + tp_collector = TPVisualizer() + tp_collector.processing(args.map_path) From 129e037c05ccd71038d18e0fca0ac280ba3aa9c1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 6 Feb 2025 03:51:19 +0000 Subject: [PATCH 7/7] style(pre-commit): autofix --- map/autoware_tp_collector/CMakeLists.txt | 8 +- .../scripts/tp_checker.py | 133 ++++++++++-------- .../scripts/tp_collector.py | 101 +++++++------ .../scripts/tp_visualizer.py | 80 ++++++----- 4 files changed, 175 insertions(+), 147 deletions(-) diff --git a/map/autoware_tp_collector/CMakeLists.txt b/map/autoware_tp_collector/CMakeLists.txt index 0a05c7c6..45de9535 100644 --- a/map/autoware_tp_collector/CMakeLists.txt +++ b/map/autoware_tp_collector/CMakeLists.txt @@ -15,10 +15,10 @@ endif () find_package(yaml-cpp REQUIRED) find_package(PCL REQUIRED) -install(PROGRAMS - scripts/tp_collector.py - scripts/tp_checker.py - scripts/tp_visualizer.py +install(PROGRAMS + scripts/tp_collector.py + scripts/tp_checker.py + scripts/tp_visualizer.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/map/autoware_tp_collector/scripts/tp_checker.py b/map/autoware_tp_collector/scripts/tp_checker.py index 43642481..d323d5dd 100755 --- a/map/autoware_tp_collector/scripts/tp_checker.py +++ b/map/autoware_tp_collector/scripts/tp_checker.py @@ -14,45 +14,44 @@ # 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 -) - -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 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 ConverterOptions +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 std_msgs.msg as std_msgs from tier4_debug_msgs.msg import Float32Stamped -from builtin_interfaces.msg import Time -import pandas as pd import tqdm -import time -import shutil -import open3d as o3d -import sensor_msgs_py.point_cloud2 as pc2 -import struct +import yaml + class TPChecker(Node): def __init__(self): - super().__init__('tp_checker') + super().__init__("tp_checker") self.segment_df = None # Segment indices, TP, kdtree - self.kdtree = None # A 2D kdtree to search for segments in the vicinity of poses + self.kdtree = None # A 2D kdtree to search for segments in the vicinity of poses self.ndt_res_df = None # A set of tp read from a rosbag - 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 # Read the input map directory and setup the segment dictionary def __set_pcd_map(self, pcd_map_dir: str): @@ -60,13 +59,13 @@ def __set_pcd_map(self, pcd_map_dir: str): if not os.path.exists(pcd_map_dir): print("Error: {0} does not exist!".format(pcd_map_dir)) exit() - + self.pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd") if not os.path.exists(self.pcd_path): print("Error: {0} does not exist!".format(self.pcd_path)) exit() - + metadata_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") if not os.path.exists(metadata_path): @@ -81,16 +80,20 @@ def __set_pcd_map(self, pcd_map_dir: str): # Read the metadata file and get the list of segments print("Read the PCD map at {0}".format(pcd_map_dir)) - self.segment_df = pd.DataFrame(columns = ["x", "y", "tp", "seg_key"]) + self.segment_df = pd.DataFrame(columns=["x", "y", "tp", "seg_key"]) with open(metadata_path, "r") as f: for key, value in yaml.safe_load(f).items(): if key != "x_resolution" and key != "y_resolution": - self.segment_df.loc[len(self.segment_df)] = [float(value[0]), float(value[1]), 0, key] - + self.segment_df.loc[len(self.segment_df)] = [ + float(value[0]), + float(value[1]), + 0, + key, + ] # Create a 2D kdtree on the segments - seg_tree_nodes = np.zeros((len(self.segment_df), 2), dtype = float) + seg_tree_nodes = np.zeros((len(self.segment_df), 2), dtype=float) for index, row in self.segment_df.iterrows(): seg_tree_nodes[index, :] = [row["x"], row["y"]] @@ -103,7 +106,6 @@ def __set_pcd_map(self, pcd_map_dir: str): for index, row in enumerate(reader): self.segment_df.loc[index, "tp"] = float(row[1]) - ##### Stamp search ##### def __stamp_search(self, stamp: int, tp_df: pd.DataFrame) -> int: @@ -127,19 +129,19 @@ def __stamp_search(self, stamp: int, tp_df: pd.DataFrame) -> int: ##### Read the input rosbag to obtain ndt pose and TP values ##### def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: 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) total_message_count = self.__compute_total_message_count(bag_path) print("Read the input rosbag at {0}".format(bag_path)) - progress_bar = tqdm.tqdm(total = total_message_count) + progress_bar = tqdm.tqdm(total=total_message_count) - pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) - tp_df = pd.DataFrame(columns = ["stamp", "tp"]) + pose_df = pd.DataFrame(columns=["stamp", "pose_msg"]) + tp_df = pd.DataFrame(columns=["stamp", "tp"]) while reader.has_next(): progress_bar.update(1) @@ -158,11 +160,11 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: progress_bar.close() # Now from the two list above build a table of estimated pose and corresponding TPs - self.ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp", "expected_tp"]) + self.ndt_res_df = pd.DataFrame(columns=["x", "y", "tp", "expected_tp"]) print("Prepare the tp list...") - progress_bar = tqdm.tqdm(total = len(pose_df)) + progress_bar = tqdm.tqdm(total=len(pose_df)) for row in pose_df.itertuples(): progress_bar.update(1) @@ -171,19 +173,23 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: pose = row.pose_msg # Find the tp whose stamp is the closest to the pose tid = self.__stamp_search(stamp, tp_df) - + if tid >= 0: - self.ndt_res_df.loc[len(self.ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid], 0.0] + self.ndt_res_df.loc[len(self.ndt_res_df)] = [ + pose.position.x, + pose.position.y, + tp_df["tp"][tid], + 0.0, + ] progress_bar.close() - # Mark the segments which may has changed def __mark_changes(self): print("Checking map changes... tp len = {0}".format(len(self.ndt_res_df))) - self.change_mark = np.zeros(len(self.segment_df), dtype = int) + self.change_mark = np.zeros(len(self.segment_df), dtype=int) - progress_bar = tqdm.tqdm(total = len(self.ndt_res_df)) + progress_bar = tqdm.tqdm(total=len(self.ndt_res_df)) for i, row in self.ndt_res_df.iterrows(): progress_bar.update(1) @@ -196,7 +202,7 @@ def __mark_changes(self): for idx in nn_idx: sum_tp += self.segment_df.loc[idx, "tp"] - + self.ndt_res_df.loc[i, "expected_tp"] = sum_tp / float(len(nn_idx)) if abs(row.tp - row.expected_tp) / row.tp >= 0.2: @@ -206,7 +212,7 @@ def __mark_changes(self): progress_bar.close() # Save the changed segments - progress_bar = tqdm.tqdm(total = len(self.change_mark)) + progress_bar = tqdm.tqdm(total=len(self.change_mark)) for idx, value in enumerate(self.change_mark): progress_bar.update(1) @@ -214,11 +220,11 @@ def __mark_changes(self): seg_key = self.segment_df.loc[idx, "seg_key"] if value != 0: - shutil.copyfile(os.path.join(self.pcd_path, seg_key), - os.path.join(self.changed_dir, seg_key)) + shutil.copyfile( + os.path.join(self.pcd_path, seg_key), os.path.join(self.changed_dir, seg_key) + ) progress_bar.close() - def __compute_total_message_count(self, rosbag_path): if rosbag_path.endswith(".db3"): yaml_path = os.path.join(os.path.dirname(rosbag_path), "metadata.yaml") @@ -234,7 +240,6 @@ def __compute_total_message_count(self, rosbag_path): return total_message_count - # Save the tp and average tps to CSV file ##### def __save_results(self: str): self.ndt_res_df.to_csv(self.result_csv) @@ -246,15 +251,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, tuple in self.segment_df.iterrows(): @@ -277,28 +288,27 @@ 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 == 1: 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 @@ -322,6 +332,7 @@ def processing(self, pcd_path: str, rosbag_path: str, result_path: str): self.__save_results() self.__show() + if __name__ == "__main__": rclpy.init() parser = argparse.ArgumentParser() diff --git a/map/autoware_tp_collector/scripts/tp_collector.py b/map/autoware_tp_collector/scripts/tp_collector.py index 3bffe207..ef703dc0 100755 --- a/map/autoware_tp_collector/scripts/tp_collector.py +++ b/map/autoware_tp_collector/scripts/tp_collector.py @@ -14,40 +14,39 @@ # 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, - SequentialWriter, - BagMetadata, - TopicMetadata, - 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 -from geometry_msgs.msg import PoseWithCovarianceStamped -from tier4_debug_msgs.msg import Float32Stamped +import os +import time + from builtin_interfaces.msg import Time -import pandas as pd -import tqdm +from geometry_msgs.msg import PoseWithCovarianceStamped +import numpy as np import open3d as o3d -import sensor_msgs.msg as sensor_msgs -import std_msgs.msg as std_msgs -import sensor_msgs_py.point_cloud2 as pc2 +import pandas as pd import rclpy from rclpy.node import Node -import time +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 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 + 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 @@ -72,31 +71,36 @@ def __compute_total_message_count(self, rosbag_path): ##### Read the YAML file to get the list of PCD segments and scores ##### def __get_pcd_segments_and_scores(self, pcd_map_dir: str): if not os.path.exists(pcd_map_dir): - print("Error: %s does not exist!"%(pcd_map_dir)) + print("Error: %s does not exist!" % (pcd_map_dir)) exit() self.pcd_path = os.path.join(pcd_map_dir, "pointcloud_map.pcd/") if not os.path.exists(self.pcd_path): - print("Error: %s does not exist!"%(self.pcd_path)) + print("Error: %s does not exist!" % (self.pcd_path)) exit() self.yaml_path = os.path.join(pcd_map_dir, "pointcloud_map_metadata.yaml") # Create a dataframe to record the avg tp of 2D segments - self.segment_df = pd.DataFrame(columns = ["x", "y", "tp", "key"]) + self.segment_df = pd.DataFrame(columns=["x", "y", "tp", "key"]) with open(self.yaml_path, "r") as f: for key, value in yaml.safe_load(f).items(): if key != "x_resolution" and key != "y_resolution": - self.segment_df.loc[len(self.segment_df)] = [float(value[0]), float(value[1]), 0, key] + self.segment_df.loc[len(self.segment_df)] = [ + float(value[0]), + float(value[1]), + 0, + key, + ] # A 2D array that contains the 2D coordinates of segments # We'll use this to build a KDtree - seg_tree_nodes = np.zeros((len(self.segment_df), 2), dtype = float) + seg_tree_nodes = np.zeros((len(self.segment_df), 2), dtype=float) for index, row in self.segment_df.iterrows(): - seg_tree_nodes[index,:] = [row["x"], row["y"]] + seg_tree_nodes[index, :] = [row["x"], row["y"]] # Create a 2D kdtree on the segment list self.kdtree = sp.KDTree(seg_tree_nodes) @@ -111,8 +115,8 @@ def __get_pcd_segments_and_scores(self, pcd_map_dir: str): self.segment_df.loc[index, "tp"] = float(row[1]) else: # If the score file does not exist, initialize scores to all 0 - self.segment_df.loc[:,"tp"] = 0 - + self.segment_df.loc[:, "tp"] = 0 + ##### Stamp search ##### def __stamp_search(self, stamp: int, tp_df: pd.DataFrame) -> int: left = 0 @@ -135,9 +139,9 @@ def __stamp_search(self, stamp: int, tp_df: pd.DataFrame) -> int: ##### Read the input rosbag to obtain ndt pose and TP values ##### def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: 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) @@ -146,16 +150,18 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: total_message_count = self.__compute_total_message_count(bag_path) progress_bar = tqdm.tqdm(total=total_message_count) - - pose_df = pd.DataFrame(columns = ["stamp", "pose_msg"]) - tp_df = pd.DataFrame(columns = ["stamp", "tp"]) + pose_df = pd.DataFrame(columns=["stamp", "pose_msg"]) + tp_df = pd.DataFrame(columns=["stamp", "tp"]) while reader.has_next(): progress_bar.update(1) (topic, data, stamp) = reader.read_next() # if topic == "/localization/pose_twist_fusion_filter/biased_pose_with_covariance": - if topic == "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias": + if ( + topic + == "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias" + ): pose_msg = deserialize_message(data, PoseWithCovarianceStamped) stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec pose_df.loc[len(pose_df)] = [stamp, pose_msg.pose.pose] @@ -164,11 +170,11 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: stamp = tp_msg.stamp.sec * 1e9 + tp_msg.stamp.nanosec tp_df.loc[len(tp_df)] = [stamp, tp_msg.data] - + progress_bar.close() - + # Now from the two list above build a table of estimated pose and corresponding TPs - ndt_res_df = pd.DataFrame(columns = ["x", "y", "tp"]) + ndt_res_df = pd.DataFrame(columns=["x", "y", "tp"]) progress_bar = tqdm.tqdm(total=len(pose_df)) @@ -178,10 +184,14 @@ def __collect_rosbag_tp(self, bag_path: str) -> pd.DataFrame: pose = row.pose_msg # Find the tp whose stamp is the closest to the pose tid = self.__stamp_search(stamp, tp_df) - + if tid >= 0: - ndt_res_df.loc[len(ndt_res_df)] = [pose.position.x, pose.position.y, tp_df["tp"][tid]] - + ndt_res_df.loc[len(ndt_res_df)] = [ + pose.position.x, + pose.position.y, + tp_df["tp"][tid], + ] + progress_bar.close() return ndt_res_df @@ -227,6 +237,7 @@ def processing(self, pcd_map_dir: str, rosbag_path: str): # Save the new TPs self.__save_tps() + if __name__ == "__main__": rclpy.init() parser = argparse.ArgumentParser() diff --git a/map/autoware_tp_collector/scripts/tp_visualizer.py b/map/autoware_tp_collector/scripts/tp_visualizer.py index 56846fae..2e15ddee 100644 --- a/map/autoware_tp_collector/scripts/tp_visualizer.py +++ b/map/autoware_tp_collector/scripts/tp_visualizer.py @@ -14,41 +14,40 @@ # 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, - SequentialWriter, - BagMetadata, - TopicMetadata, - 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 -from geometry_msgs.msg import PoseWithCovarianceStamped -from tier4_debug_msgs.msg import Float32Stamped +import os +import struct +import time + from builtin_interfaces.msg import Time -import pandas as pd -import tqdm +from geometry_msgs.msg import PoseWithCovarianceStamped +import numpy as np import open3d as o3d -import sensor_msgs.msg as sensor_msgs -import std_msgs.msg as std_msgs -import sensor_msgs_py.point_cloud2 as pc2 +import pandas as pd import rclpy from rclpy.node import Node -import time -import struct +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 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 + class TPVisualizer(Node): def __init__(self): - super().__init__('tp_visualizer') + super().__init__("tp_visualizer") self.pcd_path = None self.yaml_path = None self.score_path = None @@ -79,15 +78,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 tuple in self.segment_df.itertuples(): @@ -113,15 +118,16 @@ 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: - color_code = int(score / 7.0 * 10.0) + color_code = int(score / 7.0 * 10.0) step = 25 r = 255 - (step * color_code) @@ -129,18 +135,18 @@ def __set_color_based_on_score(self, score) -> int: 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_map_dir: str): # Get the segment lists and scores self.__get_pcd_segments_and_scores(pcd_map_dir) # Publish to rviz self.__show() + if __name__ == "__main__": rclpy.init() parser = argparse.ArgumentParser()