feat: add tools to collect and check average TPs of PCD maps

map/autoware_tp_manager/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_tp_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Find packages
+find_package(yaml-cpp REQUIRED)
+find_package(PCL REQUIRED)
+
+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()

map/autoware_tp_manager/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.

map/autoware_tp_manager/README.md

@@ -0,0 +1,83 @@
+# autoware_tp_manager
+
+Here are some tools for collecting average TPs of PCD maps. Currently, we consider the decrease of TPs as a sign of map decay. However, we don't know what TPs are 'abnormal', e.g. in some areas the TPs range around 2.0 ~ 3.0, while in others TPs float around 5.0. This package provides some tools to check it, including:
+
+- TP collector: collect the average TPs per segment of a PCD map
+- TP checker: compare a rosbag's TPs with a map's TPs and highlight the map areas where the rosbag's TPs differ significantly from the map's TPs.
+
+## Installation
+
+```bash
+cd <PATH_TO_pilot-auto.*> # OR <PATH_TO_autoware>
+cd src/
+git clone [email protected]:autowarefoundation/autoware_tools.git
+cd ..
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests --symlink-install --packages-up-to autoware_pointcloud_merger
+```
+
+## Usage
+
+- Collect the average TPs per segment from a map by TP_collector
+
+  ```bash
+  ros2 run autoware_tp_manager tp_collector.py <path_to_pcd_dir> <path_to_rosbag> <path_to_output_dir> [--resolution:=<resolution>] [--pose_topic:=<topic_of_poses>] [--tp_topic:=<topic_of_TPs>] [--scan_topic:=<topic_of_scans>]
+  ```
+
+  | Name               | Description                                                                  |
+  | ------------------ | ---------------------------------------------------------------------------- |
+  | path_to_pcd_dir    | Directory that contains the input PCD files                                  |
+  | path_to_rosbag     | Path to the input rosbag                                                     |
+  | path_to_output_dir | Path to the output directory                                                 |
+  | resolution         | Resolution to segment the input PCD. The TPs are collected on these segments |
+  | topic_of_poses     | Topic of poses messages in the input rosbag                                  |
+  | topic_of_TPs       | Topic of TPs in the input rosbag                                             |
+  | topic_of_scans     | Topic of downsampled scans in the input rosbag                               |
+
+  Paths to folders should be specified as **absolute paths**.
+
+  The rosbag should contain the following topics
+
+  - /localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias
+  - /localization/pose_estimator/transform_probability
+  - /localization/util/downsample/pointcloud
+
+  The average TPs can be visualized on Rviz2 by running the following command
+
+  ```bash
+  python3 install/autoware_tp_manager/lib/autoware_tp_manager/tp_visualizer.py <path_to_output_dir>
+  ```
+
+  | Name               | Description                                  |
+  | ------------------ | -------------------------------------------- |
+  | path_to_output_dir | Path to the output directory of TP_collector |
+
+  then open another terminal to launch Rviz2 and add the topic /autoware_tp_visualizer.
+
+- Compare a rosbags' TPs with a map's TPs by TP_checker
+
+  ```bash
+  ros2 run autoware_tp_manager tp_checker.py <path_to_score_dir> <path_to_rosbag> [--pose_topic:=<topic_of_poses>] [--tp_topic:=<topic_of_TPs>] [--scan_topic:=<topic_of_scans>]
+  ```
+
+  | Name              | Description                                                                                                               |
+  | ----------------- | ------------------------------------------------------------------------------------------------------------------------- |
+  | path_to_score_dir | Directory that contains the TP file (.csv) and the downsampled PCD map. This is the output directory of the tp_collector. |
+  | path_to_rosbag    | Path to the input rosbag to be evaluated                                                                                  |
+  | topic_of_poses    | Topic of poses in the evaluation rosbag                                                                                   |
+  | topic_of_TPs      | Topic of TPs in the evaluation rosbag                                                                                     |
+  | topic_of_scans    | Topic of scans in the evaluation rosbag                                                                                   |
+
+  The results of checking are published to the topic /autoware_tp_checker, and can also be displayed on Rviz2. The red points
+
+- The rosbags used to both TP collector and TP checker is created by running Autoware's logging simulator and record the following three topics:
+  - /localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias
+  - /localization/pose_estimator/transform_probability
+  - /localization/util/downsample/pointcloud
+
+## Parameter
+
+{{ json_to_markdown("map/autoware_tp_manager/schema/tp_manager.schema.json") }}
+
+## LICENSE
+
+This package is under [Apache License 2.0](../../LICENSE)

map/autoware_tp_manager/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_tp_manager</name>
+  <version>0.1.0</version>
+  <description>A package for checking TP scores of NDT matching</description>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <maintainer email="[email protected]">Anh Nguyen</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Anh Nguyen</author>
+  <author email="[email protected]">Masahiro Sakamoto</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>tier4_debug_msgs</buildtool_depend>
+
+  <depend>libpcl-all-dev</depend>
+  <depend>tier4_debug_msgs</depend>
+  <depend>yaml-cpp</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

map/autoware_tp_manager/schema/tp_manager.schema.json

@@ -0,0 +1,107 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for autoware tp manager node",
+  "type": "object",
+  "definitions": {
+    "tp_collector": {
+      "type": "object",
+      "properties": {
+        "path_to_pcd_dir": {
+          "type": "string",
+          "description": "The path to the directory containing the input PCD files",
+          "default": ""
+        },
+        "path_to_rosbag": {
+          "type": "string",
+          "description": "The path to the input rosbag",
+          "default": ""
+        },
+        "path_to_output_dir": {
+          "type": "string",
+          "description": "The path to the output directory",
+          "default": ""
+        },
+        "resolution": {
+          "type": "number",
+          "description": "Resolution to segment the input PCD. The TPs are collected on these segments.",
+          "default": "5,0"
+        },
+        "pose_topic": {
+          "type": "string",
+          "description": "Topic of poses messages in the input rosbag",
+          "default": "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"
+        },
+        "tp_topic": {
+          "type": "string",
+          "description": "Topic of TPs in the input rosbag",
+          "default": "/localization/pose_estimator/transform_probability"
+        },
+        "scan_topic": {
+          "type": "string",
+          "description": "Topic of downsampled scans in the input rosbag",
+          "default": "/localization/util/downsample/pointcloud"
+        }
+      },
+      "required": ["path_to_pcd_dir", "path_to_rosbag", "path_to_output_dir"],
+      "additionalProperties": false
+    },
+    "tp_checker": {
+      "type": "object",
+      "properties": {
+        "path_to_score_dir": {
+          "type": "string",
+          "description": "The path to the directory containing the average TP file (.csv) and the downsampled PCD map. This is also the output directory of the TP collector.",
+          "default": ""
+        },
+        "path_to_rosbag": {
+          "type": "string",
+          "description": "Path to the input rosbag to be evaluated",
+          "default": ""
+        },
+        "pose_topic": {
+          "type": "string",
+          "description": "Topic of poses messages in the input rosbag",
+          "default": "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"
+        },
+        "tp_topic": {
+          "type": "string",
+          "description": "Topic of TPs in the input rosbag",
+          "default": "/localization/pose_estimator/transform_probability"
+        },
+        "scan_topic": {
+          "type": "string",
+          "description": "Topic of downsampled scans in the input rosbag",
+          "default": "/localization/util/downsample/pointcloud"
+        }
+      },
+      "required": ["path_to_score_dir", "path_to_rosbag"],
+      "additionalProperties": false
+    },
+    "tp_visualizer": {
+      "type": "object",
+      "properties": {
+        "path_to_output_dir": {
+          "type": "string",
+          "description": "Path to the output directory of TP_collector",
+          "default": ""
+        }
+      },
+      "required": ["path_to_output_dir"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/autoware_pointcloud_merger"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}