feat: add tools to collect and check average TPs of PCD maps #213
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,9 +1,25 @@ | ||
| <launch> | ||
| <arg name="config_file_path" default="$(find-pkg-share autoware_pointcloud_divider)/config/pointcloud_divider.param.yaml" description="Path to the configuration YAML file"/> | ||
| <arg name="use_large_grid" default="false" description="True: group PCD files into folders"/> | ||
| <arg name="leaf_size" default="0.2" description="Downsampling resolution"/> | ||
| <arg name="grid_size_x" default="20.0" description="The X size of a segment"/> | ||
| <arg name="grid_size_y" default="20.0" description="The Y size of a segment"/> | ||
| <arg name="input_pcd_or_dir" description="The path to the folder containing the input PCD files or the input PCD file"/> | ||
| <arg name="output_pcd_dir" description="The path to the folder containing the output PCD files and metadata files"/> | ||
| <arg name="prefix" default="" description="The prefix for output PCD files"/> | ||
| <arg name="point_type" default="point_xyzi" description="The type of map points"/> | ||
|
|
||
| <group> | ||
| <node pkg="autoware_pointcloud_divider" exec="autoware_pointcloud_divider_node" name="pointcloud_divider" output="screen"> | ||
| <param from="$(var config_file_path)" allow_substs="true"/> | ||
| <param name="use_large_grid" value="$(var use_large_grid)"/> | ||
| <param name="leaf_size" value="$(var leaf_size)"/> | ||
| <param name="grid_size_x" value="$(var grid_size_x)"/> | ||
| <param name="grid_size_y" value="$(var grid_size_y)"/> | ||
| <param name="input_pcd_or_dir" value="$(var input_pcd_or_dir)"/> | ||
| <param name="output_pcd_dir" value="$(var output_pcd_dir)"/> | ||
| <param name="prefix" value="$(var prefix)"/> | ||
| <param name="point_type" value="$(var point_type)"/> | ||
| </node> | ||
| </group> | ||
| </launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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. |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,85 @@ | ||
| # 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>] [--radius <radius>] [--drop_num <drop_num>] | ||
| ``` | ||
|
|
||
| | 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 | | ||
| | radius | The radius to query map segments in vicinity of poses. This is used when scan data is not available in the rosbag. | | ||
| | drop_num | When the number of continuous low-TP poses exceeds this number, the tool stops checking because the localization is not reliable anymore | | ||
|
|
||
| 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 for 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 [optional] | ||
| - /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) | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> | ||
|
There was a problem hiding this comment. I would like to have it made with |
||
|
|
||
| <depend>libpcl-all-dev</depend> | ||
| <depend>tier4_debug_msgs</depend> | ||
| <depend>yaml-cpp</depend> | ||
|
|
||
| <export> | ||
| <build_type>ament_cmake</build_type> | ||
| </export> | ||
| </package> | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
| } |
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Could you add the python requirements?