From 9b8fff464f49c4443d857d8513f357155089e552 Mon Sep 17 00:00:00 2001 From: Shintaro SAKODA Date: Thu, 14 Dec 2023 14:17:17 +0900 Subject: [PATCH 1/6] Removed use_dynamic_map_loading Signed-off-by: Shintaro SAKODA --- localization/ndt_scan_matcher/CMakeLists.txt | 1 - localization/ndt_scan_matcher/README.md | 15 +++--- .../config/ndt_scan_matcher.param.yaml | 3 -- .../include/ndt_scan_matcher/map_module.hpp | 49 ------------------- .../ndt_scan_matcher_core.hpp | 4 -- .../ndt_scan_matcher/src/map_module.cpp | 49 ------------------- .../src/ndt_scan_matcher_core.cpp | 13 ++--- 7 files changed, 9 insertions(+), 125 deletions(-) delete mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp delete mode 100644 localization/ndt_scan_matcher/src/map_module.cpp diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 2ad4a47b833d1..b4d656cb1810b 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -30,7 +30,6 @@ ament_auto_add_executable(ndt_scan_matcher src/particle.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/map_module.cpp src/map_update_module.cpp ) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d7a7b1c5c37f3..97abde5d98c14 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -215,7 +215,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma | Name | Type | Description | | ------------------------------------- | ------ | -------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | | `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | | `dynamic_map_loading_map_radius` | double | Map loading radius for every update | | `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | @@ -235,14 +234,12 @@ Note that the dynamic map loading may FAIL if the map is split into two or more Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | -| :------------: | :-----------------------: | :------------------------: | :------------------: | -| single file | true | true | at once (standard) | -| single file | true | false | **does NOT work** | -| single file | false | true/false | at once (standard) | -| multiple files | true | true | dynamically | -| multiple files | true | false | **does NOT work** | -| multiple files | false | true/false | at once (standard) | +| PCD files | `enable_differential_load` | How NDT loads map(s) | +| :------------: | :------------------------: | :------------------: | +| single file | true | at once (standard) | +| single file | false | **does NOT work** | +| multiple files | true | dynamically | +| multiple files | false | **does NOT work** | ## Scan matching score based on de-grounded LiDAR scan diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 1a6c26cd9c351..9bc62d3f919c6 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true - # Vehicle reference frame base_frame: "base_link" diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp deleted file mode 100644 index 79454e89b9ed0..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 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 NDT_SCAN_MATCHER__MAP_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_MODULE_HPP_ - -#include - -#include - -#include -#include -#include - -#include - -class MapModule -{ - using PointSource = pcl::PointXYZ; - using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; - -public: - MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group); - -private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); - - rclcpp::Subscription::SharedPtr map_points_sub_; - std::shared_ptr ndt_ptr_; - std::mutex * ndt_ptr_mutex_; -}; - -#endif // NDT_SCAN_MATCHER__MAP_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 7f4a17a65a7e1..e68cb6bf3ce7e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -19,7 +19,6 @@ #include "localization_util/smart_pose_buffer.hpp" #include "localization_util/tf2_listener_module.hpp" -#include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -206,7 +205,6 @@ class NDTScanMatcher : public rclcpp::Node bool is_activated_; std::shared_ptr tf2_listener_module_; - std::unique_ptr map_module_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; @@ -214,8 +212,6 @@ class NDTScanMatcher : public rclcpp::Node bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; - bool use_dynamic_map_loading_; - // The execution time which means probably NDT cannot matches scans properly int64_t critical_upper_bound_exe_time_ms_; }; diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp deleted file mode 100644 index d6088a1b14949..0000000000000 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 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 "ndt_scan_matcher/map_module.hpp" - -MapModule::MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) -{ - auto map_sub_opt = rclcpp::SubscriptionOptions(); - map_sub_opt.callback_group = map_callback_group; - - map_points_sub_ = node->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt); -} - -void MapModule::callback_map_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) -{ - NormalDistributionsTransform new_ndt; - new_ndt.setParams(ndt_ptr_->getParams()); - - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt.setInputTarget(map_points_ptr); - // create Thread - // detach - auto output_cloud = std::make_shared>(); - new_ndt.align(*output_cloud); - - // swap - ndt_ptr_mutex_->lock(); - *ndt_ptr_ = new_ndt; - ndt_ptr_mutex_->unlock(); -} diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 783df34e6c0f8..6c95560f98d0d 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -289,13 +289,8 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_module_ = std::make_shared(this); - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); - if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group); - } else { - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - } + map_update_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group); logger_configure_ = std::make_unique(this); } @@ -842,9 +837,7 @@ void NDTScanMatcher::service_ndt_align( // transform pose_frame to map_frame const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, *tf_pose_to_map_ptr); - if (use_dynamic_map_loading_) { - map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); - } + map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); // mutex Map std::lock_guard lock(ndt_ptr_mtx_); From b7c66d88f6f9a5270f0dd994da14cea12560c473 Mon Sep 17 00:00:00 2001 From: Shintaro SAKODA Date: Thu, 14 Dec 2023 16:04:26 +0900 Subject: [PATCH 2/6] Removed enable_differential_load option Signed-off-by: Shintaro SAKODA --- localization/ndt_scan_matcher/README.md | 16 ++++------ .../src/map_update_module.cpp | 5 +--- map/map_loader/README.md | 5 +--- .../config/pointcloud_map_loader.param.yaml | 1 - .../pointcloud_map_loader_node.cpp | 30 ++++++++----------- 5 files changed, 19 insertions(+), 38 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 97abde5d98c14..2775c833e3fa8 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -221,11 +221,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Enabling the dynamic map loading feature -To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. -Follow the next two instructions. - -1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) -2. split the PCD files into grids (recommended size: 20[m] x 20[m]) +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m]) Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of @@ -234,12 +230,10 @@ Note that the dynamic map loading may FAIL if the map is split into two or more Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `enable_differential_load` | How NDT loads map(s) | -| :------------: | :------------------------: | :------------------: | -| single file | true | at once (standard) | -| single file | false | **does NOT work** | -| multiple files | true | dynamically | -| multiple files | false | **does NOT work** | +| PCD files | How NDT loads map(s) | +| :------------: | :------------------: | +| single file | at once (standard) | +| multiple files | dynamically | ## Scan matching score based on de-grounded LiDAR scan diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index f0a583417fb76..2ec5523d93eeb 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -110,10 +110,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->cached_ids = ndt_ptr_->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); + RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); } // send a request to map_loader diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 65d9594cb7415..fbe019096a3e7 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,7 +20,7 @@ NOTE: **We strongly recommend to use divided maps when using large pointcloud ma #### Prerequisites on pointcloud map file(s) -You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. @@ -29,8 +29,6 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. -Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). - #### Metadata structure The metadata should look like this: @@ -118,7 +116,6 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | enable_selected_load | bool | A flag to enable selected pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | | pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index ba4c032d3e514..b4efbec9706b4 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,7 +3,6 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: true - enable_differential_load: true enable_selected_load: false # only used when downsample_whole_load enabled diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index a2d9307084545..5f4b3e311e6e9 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -53,7 +53,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_differential_load = declare_parameter("enable_differential_load"); bool enable_selected_load = declare_parameter("enable_selected_load"); if (enable_whole_load) { @@ -68,26 +67,21 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load || enable_differential_load || enable_selected_load) { - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict); - } + differential_map_loader_ = std::make_unique(this, pcd_metadata_dict); - if (enable_selected_load) { - selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_selected_load) { + selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); } } From 6fd47e5273b20402b92ac2e401dae214d66847cc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 14 Dec 2023 07:44:18 +0000 Subject: [PATCH 3/6] style(pre-commit): autofix --- localization/ndt_scan_matcher/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 2775c833e3fa8..43728e83eeeb3 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -213,11 +213,11 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | -------------------------------------------------------------------- | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------ | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | ### Enabling the dynamic map loading feature From a74c1ff2b1c5af5578ae94a4a1236c9c9570891c Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 28 Dec 2023 14:24:44 +0900 Subject: [PATCH 4/6] Fixed title Signed-off-by: Shintaro Sakoda --- localization/ndt_scan_matcher/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 43728e83eeeb3..5738d23de7db2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -219,7 +219,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma | `dynamic_map_loading_map_radius` | double | Map loading radius for every update | | `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | -### Enabling the dynamic map loading feature +### Notes for dynamic map loading To use dynamic map loading feature for `ndt_scan_matcher`, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m]) From 669ce13e6bbc6019d739c86bf5111d076cbaf5a0 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 28 Dec 2023 14:28:31 +0900 Subject: [PATCH 5/6] Emphasized lock scope Signed-off-by: Shintaro Sakoda --- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index fa12b4cc26f05..109fe7b9c1926 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -402,8 +402,11 @@ void NDTScanMatcher::callback_initial_pose( << ". Please check the frame_id in the input topic and ensure it is correct."); } - std::lock_guard lock(latest_ekf_position_mtx_); - latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position; + { + // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock + std::lock_guard lock(latest_ekf_position_mtx_); + latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position; + } } void NDTScanMatcher::callback_regularization_pose( From 2a6fda6abd87874aa8fefee4d031a6fb486cd3e0 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 28 Dec 2023 14:55:34 +0900 Subject: [PATCH 6/6] Removed pointcloud_map and input_ekf_odom Signed-off-by: Shintaro Sakoda --- localization/ndt_scan_matcher/README.md | 7 ------- .../ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml | 2 -- 2 files changed, 9 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 5738d23de7db2..95a7cebdc01c8 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -18,7 +18,6 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | ----------------------------------- | ----------------------------------------------- | ------------------------------------- | | `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose | -| `pointcloud_map` | `sensor_msgs::msg::PointCloud2` | map pointcloud | | `points_raw` | `sensor_msgs::msg::PointCloud2` | sensor pointcloud | | `sensing/gnss/pose_with_covariance` | `sensor_msgs::msg::PoseWithCovarianceStamped` | base position for regularization term | @@ -193,12 +192,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Additional interfaces -#### Additional inputs - -| Name | Type | Description | -| ---------------- | ------------------------- | ----------------------------------------------------------- | -| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | - #### Additional outputs | Name | Type | Description | diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 655aa8e17ccd9..891d172aaf8fd 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -4,7 +4,6 @@ - @@ -19,7 +18,6 @@ -