diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 0f5b761baf6f0..0a04f1bb60466 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_executable(ndt_scan_matcher src/tf2_listener_module.cpp src/map_module.cpp src/pose_initialization_module.cpp + src/map_update_module.cpp ) link_directories(${PCL_LIBRARY_DIRS}) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 81fac59e4b8db..7c63785aebcd2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -64,7 +64,6 @@ One optional function is regularization. Please see the regularization chapter i | `max_iterations` | int | The number of iterations required to calculate alignment | | `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | -| `neighborhood_search_method` | int | neighborhood search method (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1) | | `num_threads` | int | Number of threads used for parallel computing | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) @@ -172,6 +171,67 @@ The color of the trajectory indicates the error (meter) from the reference traje drawing drawing +## Dynamic map loading + +Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion. + +Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) + +drawing + +### 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 | +| ----------------------------- | ------------------------------- | ------------------------------------------------- | +| `debug/loaded_pointcloud_map` | `sensor_msgs::msg::PointCloud2` | pointcloud maps used for localization (for debug) | + +#### Additional client + +| Name | Type | Description | +| ------------------- | ------------------------------------------------------ | ------------------ | +| `client_map_loader` | `autoware_map_msgs::srv::GetDifferentialPointCloudMap` | map loading client | + +### Parameters + +| 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) | + +### 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]) + +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 + +- one PCD map file +- multiple PCD map files divided into small size (~20[m]) + +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) | +| splitted | true | true | dynamically | +| splitted | true | false | **does NOT work** | +| splitted | false | true/false | at once (standard) | + ## Scan matching score based on de-grounded LiDAR scan ### Abstract 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 1fb8fd003b4ac..56c5baa347aaa 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # Use dynamic map loading + use_dynamic_map_loading: true # Vehicle reference frame base_frame: "base_link" @@ -41,10 +43,6 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - # Number of threads used for parallel computing num_threads: 4 @@ -63,9 +61,18 @@ # Regularization switch regularization_enabled: false - # Regularization scale factor + # regularization scale factor regularization_scale_factor: 0.01 + # Dynamic map loading distance + dynamic_map_loading_update_distance: 20.0 + + # Dynamic map loading loading radius + dynamic_map_loading_map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 + # A flag for using scan matching score based on de-grounded LiDAR scan estimate_scores_for_degrounded_scan: false 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 index 871ff2760acf7..5253879a28937 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -19,9 +19,9 @@ #include +#include #include #include -#include #include @@ -30,7 +30,7 @@ class MapModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: MapModule( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp new file mode 100644 index 0000000000000..fb9577ca42934 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -0,0 +1,107 @@ +// 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_UPDATE_MODULE_HPP_ +#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ + +#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/particle.hpp" +#include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "ndt_scan_matcher/util_func.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +class MapUpdateModule +{ + using PointSource = pcl::PointXYZ; + using PointTarget = pcl::PointXYZ; + using NormalDistributionsTransform = + pclomp::MultiGridNormalDistributionsTransform; + +public: + MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + std::shared_ptr> state_ptr); + +private: + void service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); + void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); + void map_update_timer_callback(); + + void update_ndt( + const std::vector & maps_to_add, + const std::vector & map_ids_to_remove); + void update_map(const geometry_msgs::msg::Point & position); + bool should_update_map(const geometry_msgs::msg::Point & position) const; + void publish_partial_pcd_map(); + geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); + void publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr); + + rclcpp::Publisher::SharedPtr loaded_pcd_pub_; + rclcpp::Publisher::SharedPtr + ndt_monte_carlo_initial_pose_marker_pub_; + rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; + + rclcpp::Service::SharedPtr service_; + rclcpp::Client::SharedPtr + pcd_loader_client_; + rclcpp::TimerBase::SharedPtr map_update_timer_; + + rclcpp::Subscription::SharedPtr ekf_odom_sub_; + + rclcpp::CallbackGroup::SharedPtr map_callback_group_; + + std::shared_ptr ndt_ptr_; + std::mutex * ndt_ptr_mutex_; + std::string map_frame_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr tf2_listener_module_; + std::shared_ptr> state_ptr_; + + int initial_estimate_particles_num_; + std::optional last_update_position_ = std::nullopt; + std::optional current_position_ = std::nullopt; + const double dynamic_map_loading_update_distance_; + const double dynamic_map_loading_map_radius_; + const double lidar_radius_; +}; + +#endif // NDT_SCAN_MATCHER__MAP_UPDATE_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 30356bc61cee0..1bd7a509a3a7b 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 @@ -18,6 +18,7 @@ #define FMT_HEADER_ONLY #include "ndt_scan_matcher/map_module.hpp" +#include "ndt_scan_matcher/map_update_module.hpp" #include "ndt_scan_matcher/pose_initialization_module.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" @@ -34,8 +35,8 @@ #include #include +#include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -71,7 +72,7 @@ class NDTScanMatcher : public rclcpp::Node using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: NDTScanMatcher(); @@ -200,6 +201,7 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr tf2_listener_module_; std::unique_ptr map_module_; std::unique_ptr pose_init_module_; + std::unique_ptr map_update_module_; bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp index 7745ecf8a2148..0e1a6e6816413 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp @@ -27,8 +27,8 @@ #include #include +#include #include -#include #include #include @@ -39,7 +39,7 @@ class PoseInitializationModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: PoseInitializationModule( 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 9e3d966bd770a..b442ac1b3d843 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -7,10 +7,13 @@ + + + @@ -23,6 +26,9 @@ + + + diff --git a/localization/ndt_scan_matcher/media/differential_area_loading.gif b/localization/ndt_scan_matcher/media/differential_area_loading.gif new file mode 100644 index 0000000000000..4283b738a1dfa Binary files /dev/null and b/localization/ndt_scan_matcher/media/differential_area_loading.gif differ diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 13b0f186ceb98..581879322caf3 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_map_msgs diagnostic_msgs fmt geometry_msgs diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp new file mode 100644 index 0000000000000..2e8052b41d413 --- /dev/null +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -0,0 +1,299 @@ +// 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_update_module.hpp" + +template +double norm_xy(const T p1, const U p2) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + return std::sqrt(dx * dx + dy * dy); +} + +MapUpdateModule::MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + std::shared_ptr> state_ptr) +: ndt_ptr_(ndt_ptr), + ndt_ptr_mutex_(ndt_ptr_mutex), + map_frame_(map_frame), + logger_(node->get_logger()), + clock_(node->get_clock()), + tf2_listener_module_(tf2_listener_module), + state_ptr_(state_ptr), + dynamic_map_loading_update_distance_( + node->declare_parameter("dynamic_map_loading_update_distance")), + dynamic_map_loading_map_radius_( + node->declare_parameter("dynamic_map_loading_map_radius")), + lidar_radius_(node->declare_parameter("lidar_radius")) +{ + initial_estimate_particles_num_ = node->declare_parameter("initial_estimate_particles_num"); + + sensor_aligned_pose_pub_ = + node->create_publisher("monte_carlo_points_aligned", 10); + ndt_monte_carlo_initial_pose_marker_pub_ = + node->create_publisher( + "monte_carlo_initial_pose_marker", 10); + + auto main_sub_opt = rclcpp::SubscriptionOptions(); + main_sub_opt.callback_group = main_callback_group; + + map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + ekf_odom_sub_ = node->create_subscription( + "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), + main_sub_opt); + + loaded_pcd_pub_ = node->create_publisher( + "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); + + service_ = node->create_service( + "ndt_align_srv", + std::bind( + &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_); + + pcd_loader_client_ = node->create_client( + "pcd_loader_service", rmw_qos_profile_services_default); + 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`."); + } + + double map_update_dt = 1.0; + auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), + map_callback_group_); +} + +void MapUpdateModule::service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) +{ + // get TF from pose_frame to map_frame + auto TF_pose_to_map_ptr = std::make_shared(); + tf2_listener_module_->get_transform( + clock_->now(), map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr); + + // transform pose_frame to map_frame + const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); + update_map(mapTF_initial_pose_msg.pose.pose.position); + + if (ndt_ptr_->getInputTarget() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputTarget"); + return; + } + + if (ndt_ptr_->getInputSource() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputSource"); + return; + } + + // mutex Map + std::lock_guard lock(*ndt_ptr_mutex_); + + (*state_ptr_)["state"] = "Aligning"; + res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, mapTF_initial_pose_msg); + (*state_ptr_)["state"] = "Sleeping"; + res->success = true; + res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; + + last_update_position_ = res->pose_with_covariance.pose.pose.position; +} + +void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) +{ + current_position_ = odom_ptr->pose.pose.position; + + if (last_update_position_ == std::nullopt) { + return; + } + double distance = norm_xy(current_position_.value(), last_update_position_.value()); + if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); + } +} + +void MapUpdateModule::map_update_timer_callback() +{ + if (current_position_ == std::nullopt) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 1, + "Cannot find the reference position for map update. Please check if the EKF odometry is " + "provided to NDT."); + return; + } + if (last_update_position_ == std::nullopt) return; + + // continue only if we should update the map + if (should_update_map(current_position_.value())) { + RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); + update_map(current_position_.value()); + last_update_position_ = current_position_; + } +} + +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const +{ + if (last_update_position_ == std::nullopt) return false; + double distance = norm_xy(position, last_update_position_.value()); + return distance > dynamic_map_loading_update_distance_; +} + +void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) +{ + auto request = std::make_shared(); + request->area.center = position; + request->area.radius = dynamic_map_loading_map_radius_; + request->cached_ids = ndt_ptr_->getCurrentMapIDs(); + + // // send a request to map_loader + auto result{pcd_loader_client_->async_send_request( + request, + [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(logger_, "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } + update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); +} + +void MapUpdateModule::update_ndt( + const std::vector & maps_to_add, + const std::vector & map_ids_to_remove) +{ + RCLCPP_INFO( + logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); + if (maps_to_add.empty() && map_ids_to_remove.empty()) { + RCLCPP_INFO(logger_, "Skip map update"); + return; + } + const auto exe_start_time = std::chrono::system_clock::now(); + + NormalDistributionsTransform backup_ndt = *ndt_ptr_; + + // Add pcd + for (const auto & map_to_add : maps_to_add) { + pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); + pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); + backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + } + + // Remove pcd + for (const std::string & map_id_to_remove : map_ids_to_remove) { + backup_ndt.removeTarget(map_id_to_remove); + } + + backup_ndt.createVoxelKdtree(); + + const auto exe_end_time = std::chrono::system_clock::now(); + const double exe_time = + std::chrono::duration_cast(exe_end_time - exe_start_time).count() / + 1000.0; + RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + + // swap + (*ndt_ptr_mutex_).lock(); + // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should + // ideally be avoided. But I will leave this for now since I cannot come up with a solution other + // than using pointer of pointer. + *ndt_ptr_ = backup_ndt; + (*ndt_ptr_mutex_).unlock(); + + publish_partial_pcd_map(); +} + +geometry_msgs::msg::PoseWithCovarianceStamped MapUpdateModule::align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) +{ + if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { + RCLCPP_WARN(logger_, "No Map or Sensor PointCloud"); + return geometry_msgs::msg::PoseWithCovarianceStamped(); + } + + // generateParticle + const auto initial_poses = + create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); + + std::vector particle_array; + auto output_cloud = std::make_shared>(); + + for (unsigned int i = 0; i < initial_poses.size(); i++) { + const auto & initial_pose = initial_poses[i]; + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); + ndt_ptr->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); + + Particle particle( + initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, + ndt_result.iteration_num); + particle_array.push_back(particle); + const auto marker_array = make_debug_markers( + clock_->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, + i); + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + + auto sensor_points_mapTF_ptr = std::make_shared>(); + pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); + publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); + } + + auto best_particle_ptr = std::max_element( + std::begin(particle_array), std::end(particle_array), + [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); + + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; + result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; + + return result_pose_with_cov_msg; +} + +void MapUpdateModule::publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr) +{ + sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; + pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); + sensor_points_mapTF_msg.header.stamp = sensor_ros_time; + sensor_points_mapTF_msg.header.frame_id = frame_id; + sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); +} + +void MapUpdateModule::publish_partial_pcd_map() +{ + pcl::PointCloud map_pcl = ndt_ptr_->getVoxelPCD(); + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(map_pcl, map_msg); + map_msg.header.frame_id = "map"; + + loaded_pcd_pub_->publish(map_msg); +} 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 877ffef7908dc..a9c37d7843a64 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -116,8 +116,6 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); ndt_params.max_iterations = this->declare_parameter("max_iterations"); - int search_method = this->declare_parameter("neighborhood_search_method"); - ndt_params.search_method = static_cast(search_method); ndt_params.num_threads = this->declare_parameter("num_threads"); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = @@ -222,10 +220,17 @@ NDTScanMatcher::NDTScanMatcher() diagnostic_thread_.detach(); tf2_listener_module_ = std::make_shared(this); - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - pose_init_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, - state_ptr_); + + if (this->declare_parameter("use_dynamic_map_loading")) { + map_update_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); + } else { + map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); + pose_init_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); + } } void NDTScanMatcher::timer_diagnostic() diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index 8f3ccbff00360..61b02c490c663 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -2,8 +2,8 @@ ros__parameters: enable_whole_load: true enable_downsampled_whole_load: false - enable_partial_load: false - enable_differential_load: false + enable_partial_load: true + enable_differential_load: true # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m]