From 184eb9f013b94bebc23923b1fa8eaffff8199c3c Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 8 Nov 2022 12:43:10 +0900 Subject: [PATCH 1/9] still work in progress Signed-off-by: kminoda --- localization/ndt_scan_matcher/CMakeLists.txt | 1 + .../include/ndt_scan_matcher/map_module.hpp | 46 ++++++++++++ .../ndt_scan_matcher_core.hpp | 6 +- .../ndt_scan_matcher/src/map_module.cpp | 56 ++++++++++++++ .../src/ndt_scan_matcher_core.cpp | 75 ++++++------------- 5 files changed, 130 insertions(+), 54 deletions(-) create mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp create 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 bc8c6fb12534c..0e77b65173373 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -32,6 +32,7 @@ ament_auto_add_executable(ndt_scan_matcher src/util_func.cpp src/pose_array_interpolator.cpp src/tf2_listener_module.cpp + src/map_module.cpp ) link_directories(${PCL_LIBRARY_DIRS}) 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 new file mode 100644 index 0000000000000..5f881339b540c --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -0,0 +1,46 @@ +// 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::NormalDistributionsTransform; + +public: + MapModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr> ndt_ptr_ptr, + rclcpp::CallbackGroup::SharedPtr map_callback_group); + +private: + void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + + rclcpp::Subscription::SharedPtr map_points_sub_; + std::shared_ptr> ndt_ptr_ptr_; + std::mutex * ndt_ptr_mutex_; + +}; + +#endif // NDT_SCAN_MATCHER__MAP_MODULE_HPP_ \ No newline at end of file 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 20548a1f21d38..1640512483f5e 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,6 +19,7 @@ #include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "ndt_scan_matcher/map_module.hpp" #include #include @@ -95,7 +96,6 @@ class NDTScanMatcher : public rclcpp::Node const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); void callback_sensor_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); void callback_initial_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); @@ -141,7 +141,6 @@ class NDTScanMatcher : public rclcpp::Node void timer_diagnostic(); rclcpp::Subscription::SharedPtr initial_pose_sub_; - rclcpp::Subscription::SharedPtr map_points_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; rclcpp::Subscription::SharedPtr regularization_pose_sub_; @@ -173,7 +172,7 @@ class NDTScanMatcher : public rclcpp::Node tf2_ros::TransformBroadcaster tf2_broadcaster_; - std::shared_ptr ndt_ptr_; + std::shared_ptr> ndt_ptr_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; std::string base_frame_; @@ -208,6 +207,7 @@ class NDTScanMatcher : public rclcpp::Node bool is_activated_; std::shared_ptr tf2_listener_module_; + std::unique_ptr map_module_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp new file mode 100644 index 0000000000000..f46fe649c40e1 --- /dev/null +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -0,0 +1,56 @@ +// 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_ptr, + rclcpp::CallbackGroup::SharedPtr map_callback_group) +: ndt_ptr_ptr_(ndt_ptr_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) +{ + std::shared_ptr new_ndt_ptr(new NormalDistributionsTransform); + new_ndt_ptr->setTransformationEpsilon(ndt_params_.trans_epsilon); + new_ndt_ptr->setStepSize(ndt_params_.step_size); + new_ndt_ptr->setResolution(ndt_params_.resolution); + new_ndt_ptr->setMaximumIterations(ndt_params_.max_iterations); + new_ndt_ptr->setRegularizationScaleFactor(ndt_params_.regularization_scale_factor); + new_ndt_ptr->setNeighborhoodSearchMethod(ndt_params_.search_method); + new_ndt_ptr->setNumThreads(ndt_params_.num_threads); + + pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); + pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); + new_ndt_ptr->setInputTarget(map_points_ptr); + // create Thread + // detach + auto output_cloud = std::make_shared>(); + new_ndt_ptr->align(*output_cloud); + + // swap + ndt_ptr_mutex_->lock(); + ndt_ptr_ = new_ndt_ptr; + 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 48e958abe6a9f..99d19dd493413 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -82,7 +82,6 @@ bool validate_local_optimal_solution_oscillation( NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), - ndt_ptr_(new NormalDistributionsTransform), base_frame_("base_link"), ndt_base_frame_("ndt_base_link"), map_frame_("map"), @@ -118,13 +117,15 @@ NDTScanMatcher::NDTScanMatcher() ndt_params_.num_threads = this->declare_parameter("num_threads"); ndt_params_.num_threads = std::max(ndt_params_.num_threads, 1); - ndt_ptr_->setTransformationEpsilon(ndt_params_.trans_epsilon); - ndt_ptr_->setStepSize(ndt_params_.step_size); - ndt_ptr_->setResolution(ndt_params_.resolution); - ndt_ptr_->setMaximumIterations(ndt_params_.max_iterations); - ndt_ptr_->setRegularizationScaleFactor(ndt_params_.regularization_scale_factor); - ndt_ptr_->setNeighborhoodSearchMethod(ndt_params_.search_method); - ndt_ptr_->setNumThreads(ndt_params_.num_threads); + std::shared_ptr ndt_ptr(new NormalDistributionsTransform); + ndt_ptr->setTransformationEpsilon(ndt_params_.trans_epsilon); + ndt_ptr->setStepSize(ndt_params_.step_size); + ndt_ptr->setResolution(ndt_params_.resolution); + ndt_ptr->setMaximumIterations(ndt_params_.max_iterations); + ndt_ptr->setRegularizationScaleFactor(ndt_params_.regularization_scale_factor); + ndt_ptr->setNeighborhoodSearchMethod(ndt_params_.search_method); + ndt_ptr->setNumThreads(ndt_params_.num_threads); + ndt_ptr_ptr_ = std::make_shared>(ndt_ptr); RCLCPP_INFO( get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", @@ -172,9 +173,6 @@ NDTScanMatcher::NDTScanMatcher() "ekf_pose_with_covariance", 100, std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); - map_points_sub_ = this->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&NDTScanMatcher::callback_map_points, this, std::placeholders::_1), main_sub_opt); sensor_points_sub_ = this->create_subscription( "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt); @@ -231,6 +229,7 @@ NDTScanMatcher::NDTScanMatcher() diagnostic_thread_.detach(); tf2_listener_module_ = std::make_shared(this); + map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_ptr_, main_callback_group); } void NDTScanMatcher::timer_diagnostic() @@ -296,13 +295,13 @@ void NDTScanMatcher::service_ndt_align( // transform pose_frame to map_frame const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); - if (ndt_ptr_->getInputTarget() == nullptr) { + if ((*ndt_ptr_ptr_)->getInputTarget() == nullptr) { res->success = false; RCLCPP_WARN(get_logger(), "No InputTarget"); return; } - if (ndt_ptr_->getInputSource() == nullptr) { + if ((*ndt_ptr_ptr_)->getInputSource() == nullptr) { res->success = false; RCLCPP_WARN(get_logger(), "No InputSource"); return; @@ -312,7 +311,7 @@ void NDTScanMatcher::service_ndt_align( std::lock_guard lock(ndt_ptr_mtx_); key_value_stdmap_["state"] = "Aligning"; - res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, mapTF_initial_pose_msg); + res->pose_with_covariance = align_using_monte_carlo(*ndt_ptr_ptr_, mapTF_initial_pose_msg); key_value_stdmap_["state"] = "Sleeping"; res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; @@ -358,32 +357,6 @@ void NDTScanMatcher::callback_regularization_pose( regularization_pose_msg_ptr_array_.push_back(pose_conv_msg_ptr); } -void NDTScanMatcher::callback_map_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) -{ - std::shared_ptr new_ndt_ptr(new NormalDistributionsTransform); - new_ndt_ptr->setTransformationEpsilon(ndt_params_.trans_epsilon); - new_ndt_ptr->setStepSize(ndt_params_.step_size); - new_ndt_ptr->setResolution(ndt_params_.resolution); - new_ndt_ptr->setMaximumIterations(ndt_params_.max_iterations); - new_ndt_ptr->setRegularizationScaleFactor(ndt_params_.regularization_scale_factor); - new_ndt_ptr->setNeighborhoodSearchMethod(ndt_params_.search_method); - new_ndt_ptr->setNumThreads(ndt_params_.num_threads); - - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt_ptr->setInputTarget(map_points_ptr); - // create Thread - // detach - auto output_cloud = std::make_shared>(); - new_ndt_ptr->align(*output_cloud); - - // swap - ndt_ptr_mtx_.lock(); - ndt_ptr_ = new_ndt_ptr; - ndt_ptr_mtx_.unlock(); -} - void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr) { @@ -403,7 +376,7 @@ void NDTScanMatcher::callback_sensor_points( pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr); transform_sensor_measurement( sensor_frame, base_frame_, sensor_points_sensorTF_ptr, sensor_points_baselinkTF_ptr); - ndt_ptr_->setInputSource(sensor_points_baselinkTF_ptr); + (*ndt_ptr_ptr_)->setInputSource(sensor_points_baselinkTF_ptr); if (!is_activated_) return; // calculate initial pose @@ -422,7 +395,7 @@ void NDTScanMatcher::callback_sensor_points( // if regularization is enabled and available, set pose to NDT for regularization if (regularization_enabled_) add_regularization_pose(sensor_ros_time); - if (ndt_ptr_->getInputTarget() == nullptr) { + if ((*ndt_ptr_ptr_)->getInputTarget() == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); return; } @@ -432,8 +405,8 @@ void NDTScanMatcher::callback_sensor_points( const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(interpolator.get_current_pose().pose.pose); auto output_cloud = std::make_shared>(); - ndt_ptr_->align(*output_cloud, initial_pose_matrix); - const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); + (*ndt_ptr_ptr_)->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = (*ndt_ptr_ptr_)->getResult(); key_value_stdmap_["state"] = "Sleeping"; const auto exe_end_time = std::chrono::system_clock::now(); @@ -450,7 +423,7 @@ void NDTScanMatcher::callback_sensor_points( // perform several validations /***************************************************************************** - The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in + The reason the add 2 to the (*ndt_ptr_ptr_)->getMaximumIterations() is that there are bugs in implementation of ndt. 1. gradient descent method ends when the iteration is greater than max_iteration if it does not converge (be careful it's 'greater than' instead of 'greater equal than'.) @@ -462,7 +435,7 @@ void NDTScanMatcher::callback_sensor_points( https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 *****************************************************************************/ bool is_ok_iteration_num = - validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations() + 2); + validate_num_iteration(ndt_result.iteration_num, (*ndt_ptr_ptr_)->getMaximumIterations() + 2); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( @@ -546,8 +519,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ 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(); + (*ndt_ptr_ptr_)->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = (*ndt_ptr_ptr_)->getResult(); Particle particle( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, @@ -639,7 +612,7 @@ void NDTScanMatcher::publish_marker( } // TODO(Tier IV): delete old marker - for (; i < ndt_ptr_->getMaximumIterations() + 2;) { + for (; i < (*ndt_ptr_ptr_)->getMaximumIterations() + 2;) { marker.id = i++; marker.pose = geometry_msgs::msg::Pose(); marker.color = exchange_color_crc(0); @@ -737,10 +710,10 @@ std::optional NDTScanMatcher::interpolate_regularization_pose( void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) { - ndt_ptr_->unsetRegularizationPose(); + (*ndt_ptr_ptr_)->unsetRegularizationPose(); std::optional pose_opt = interpolate_regularization_pose(sensor_ros_time); if (pose_opt.has_value()) { - ndt_ptr_->setRegularizationPose(pose_opt.value()); + (*ndt_ptr_ptr_)->setRegularizationPose(pose_opt.value()); RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); } } From 3ab08a53d63de2efb09d1dd77763f72c02d24e0d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 8 Nov 2022 11:36:06 +0000 Subject: [PATCH 2/9] ci(pre-commit): autofix --- .../include/ndt_scan_matcher/map_module.hpp | 12 +++++++----- .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 2 +- localization/ndt_scan_matcher/src/map_module.cpp | 4 +--- 3 files changed, 9 insertions(+), 9 deletions(-) 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 5f881339b540c..b4c8dc36415d2 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 @@ -16,13 +16,16 @@ #define NDT_SCAN_MATCHER__MAP_MODULE_HPP_ #include + #include -#include -#include + #include +#include #include +#include -class MapModule { +class MapModule +{ using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = @@ -40,7 +43,6 @@ class MapModule { rclcpp::Subscription::SharedPtr map_points_sub_; std::shared_ptr> ndt_ptr_ptr_; std::mutex * ndt_ptr_mutex_; - }; -#endif // NDT_SCAN_MATCHER__MAP_MODULE_HPP_ \ No newline at end of file +#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 9db47702e74b9..2a1216bdbc524 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 @@ -17,9 +17,9 @@ #define FMT_HEADER_ONLY +#include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" -#include "ndt_scan_matcher/map_module.hpp" #include #include diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp index b75139693b7e4..925ce37b8d011 100644 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -18,8 +18,7 @@ MapModule::MapModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr> ndt_ptr_ptr, rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_ptr_(ndt_ptr_ptr), - ndt_ptr_mutex_(ndt_ptr_mutex) +: ndt_ptr_ptr_(ndt_ptr_ptr), ndt_ptr_mutex_(ndt_ptr_mutex) { auto map_sub_opt = rclcpp::SubscriptionOptions(); map_sub_opt.callback_group = map_callback_group; @@ -35,7 +34,6 @@ void MapModule::callback_map_points( std::shared_ptr new_ndt_ptr(new NormalDistributionsTransform); new_ndt_ptr->setParams((*ndt_ptr_ptr_)->getParams()); - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); new_ndt_ptr->setInputTarget(map_points_ptr); From b70327e707706f77fc7aaea06e53cfb09d79f877 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 09:46:09 +0900 Subject: [PATCH 3/9] ptr_ptr yameru Signed-off-by: kminoda --- .../include/ndt_scan_matcher/map_module.hpp | 4 +-- .../ndt_scan_matcher_core.hpp | 2 +- .../launch/ndt_scan_matcher.launch.xml | 2 +- .../ndt_scan_matcher/src/map_module.cpp | 16 +++++----- .../src/ndt_scan_matcher_core.cpp | 31 +++++++++---------- 5 files changed, 27 insertions(+), 28 deletions(-) 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 5f881339b540c..5d41c779a9422 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 @@ -31,14 +31,14 @@ class MapModule { public: MapModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr> ndt_ptr_ptr, + std::shared_ptr ndt_ptr, rclcpp::CallbackGroup::SharedPtr map_callback_group); private: void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); rclcpp::Subscription::SharedPtr map_points_sub_; - std::shared_ptr> ndt_ptr_ptr_; + std::shared_ptr ndt_ptr_; std::mutex * ndt_ptr_mutex_; }; 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 9db47702e74b9..5d9d1f4f1861c 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 @@ -161,7 +161,7 @@ class NDTScanMatcher : public rclcpp::Node tf2_ros::TransformBroadcaster tf2_broadcaster_; - std::shared_ptr> ndt_ptr_ptr_; + std::shared_ptr ndt_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; std::string base_frame_; 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..beb0b4359abb1 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -13,7 +13,7 @@ - + diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp index b75139693b7e4..36ac18ef77e16 100644 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -16,9 +16,9 @@ MapModule::MapModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr> ndt_ptr_ptr, + std::shared_ptr ndt_ptr, rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_ptr_(ndt_ptr_ptr), +: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex) { auto map_sub_opt = rclcpp::SubscriptionOptions(); @@ -32,20 +32,20 @@ MapModule::MapModule( void MapModule::callback_map_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) { - std::shared_ptr new_ndt_ptr(new NormalDistributionsTransform); - new_ndt_ptr->setParams((*ndt_ptr_ptr_)->getParams()); - + NormalDistributionsTransform new_ndt; + new_ndt.setParams(ndt_ptr_->getParams()); + new_ndt.setNumThreads(2); pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt_ptr->setInputTarget(map_points_ptr); + new_ndt.setInputTarget(map_points_ptr); // create Thread // detach auto output_cloud = std::make_shared>(); - new_ndt_ptr->align(*output_cloud); + new_ndt.align(*output_cloud); // swap ndt_ptr_mutex_->lock(); - *ndt_ptr_ptr_ = new_ndt_ptr; + *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 2b44f4d46b9ce..4001fd7b412af 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -82,6 +82,7 @@ bool validate_local_optimal_solution_oscillation( NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), + ndt_ptr_(new NormalDistributionsTransform), base_frame_("base_link"), ndt_base_frame_("ndt_base_link"), map_frame_("map"), @@ -120,9 +121,7 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.regularization_scale_factor = this->declare_parameter("regularization_scale_factor"); - std::shared_ptr ndt_ptr(new NormalDistributionsTransform); - ndt_ptr->setParams(ndt_params); - ndt_ptr_ptr_ = std::make_shared>(ndt_ptr); + ndt_ptr_->setParams(ndt_params); RCLCPP_INFO( get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", @@ -226,7 +225,7 @@ NDTScanMatcher::NDTScanMatcher() diagnostic_thread_.detach(); tf2_listener_module_ = std::make_shared(this); - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_ptr_, main_callback_group); + map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); } void NDTScanMatcher::timer_diagnostic() @@ -292,13 +291,13 @@ void NDTScanMatcher::service_ndt_align( // transform pose_frame to map_frame const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); - if ((*ndt_ptr_ptr_)->getInputTarget() == nullptr) { + if (ndt_ptr_->getInputTarget() == nullptr) { res->success = false; RCLCPP_WARN(get_logger(), "No InputTarget"); return; } - if ((*ndt_ptr_ptr_)->getInputSource() == nullptr) { + if (ndt_ptr_->getInputSource() == nullptr) { res->success = false; RCLCPP_WARN(get_logger(), "No InputSource"); return; @@ -308,7 +307,7 @@ void NDTScanMatcher::service_ndt_align( std::lock_guard lock(ndt_ptr_mtx_); key_value_stdmap_["state"] = "Aligning"; - res->pose_with_covariance = align_using_monte_carlo(*ndt_ptr_ptr_, mapTF_initial_pose_msg); + res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, mapTF_initial_pose_msg); key_value_stdmap_["state"] = "Sleeping"; res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; @@ -373,7 +372,7 @@ void NDTScanMatcher::callback_sensor_points( pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr); transform_sensor_measurement( sensor_frame, base_frame_, sensor_points_sensorTF_ptr, sensor_points_baselinkTF_ptr); - (*ndt_ptr_ptr_)->setInputSource(sensor_points_baselinkTF_ptr); + ndt_ptr_->setInputSource(sensor_points_baselinkTF_ptr); if (!is_activated_) return; // calculate initial pose @@ -392,7 +391,7 @@ void NDTScanMatcher::callback_sensor_points( // if regularization is enabled and available, set pose to NDT for regularization if (regularization_enabled_) add_regularization_pose(sensor_ros_time); - if ((*ndt_ptr_ptr_)->getInputTarget() == nullptr) { + if (ndt_ptr_->getInputTarget() == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); return; } @@ -402,8 +401,8 @@ void NDTScanMatcher::callback_sensor_points( const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(interpolator.get_current_pose().pose.pose); auto output_cloud = std::make_shared>(); - (*ndt_ptr_ptr_)->align(*output_cloud, initial_pose_matrix); - const pclomp::NdtResult ndt_result = (*ndt_ptr_ptr_)->getResult(); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); key_value_stdmap_["state"] = "Sleeping"; const auto exe_end_time = std::chrono::system_clock::now(); @@ -420,7 +419,7 @@ void NDTScanMatcher::callback_sensor_points( // perform several validations /***************************************************************************** - The reason the add 2 to the (*ndt_ptr_ptr_)->getMaximumIterations() is that there are bugs in + The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in implementation of ndt. 1. gradient descent method ends when the iteration is greater than max_iteration if it does not converge (be careful it's 'greater than' instead of 'greater equal than'.) @@ -432,7 +431,7 @@ void NDTScanMatcher::callback_sensor_points( https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 *****************************************************************************/ bool is_ok_iteration_num = - validate_num_iteration(ndt_result.iteration_num, (*ndt_ptr_ptr_)->getMaximumIterations() + 2); + validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations() + 2); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( @@ -609,7 +608,7 @@ void NDTScanMatcher::publish_marker( } // TODO(Tier IV): delete old marker - for (; i < (*ndt_ptr_ptr_)->getMaximumIterations() + 2;) { + for (; i < ndt_ptr_->getMaximumIterations() + 2;) { marker.id = i++; marker.pose = geometry_msgs::msg::Pose(); marker.color = exchange_color_crc(0); @@ -707,10 +706,10 @@ std::optional NDTScanMatcher::interpolate_regularization_pose( void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) { - (*ndt_ptr_ptr_)->unsetRegularizationPose(); + ndt_ptr_->unsetRegularizationPose(); std::optional pose_opt = interpolate_regularization_pose(sensor_ros_time); if (pose_opt.has_value()) { - (*ndt_ptr_ptr_)->setRegularizationPose(pose_opt.value()); + ndt_ptr_->setRegularizationPose(pose_opt.value()); RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); } } From 07e54dd8f7e44e32dd697d5a4c71f422b342e46b Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 09:48:12 +0900 Subject: [PATCH 4/9] remove unnecessary newline Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 1 - 1 file changed, 1 deletion(-) 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 4001fd7b412af..88be6a8967c87 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -120,7 +120,6 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = this->declare_parameter("regularization_scale_factor"); - ndt_ptr_->setParams(ndt_params); RCLCPP_INFO( From 500081cd9956f46cf265afa6e7de1aa65cc9238f Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 09:48:37 +0900 Subject: [PATCH 5/9] revert launch setting Signed-off-by: kminoda --- .../ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 beb0b4359abb1..9e3d966bd770a 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -13,7 +13,7 @@ - + From 1fb3ab590bce0cf875e146514ce80a520a812200 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 9 Nov 2022 00:50:14 +0000 Subject: [PATCH 6/9] ci(pre-commit): autofix --- localization/ndt_scan_matcher/src/map_module.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp index 36ac18ef77e16..b584447887f3d 100644 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -18,8 +18,7 @@ MapModule::MapModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(ndt_ptr), - ndt_ptr_mutex_(ndt_ptr_mutex) +: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex) { auto map_sub_opt = rclcpp::SubscriptionOptions(); map_sub_opt.callback_group = map_callback_group; From 1514769128aabc29d116d2a463fe67edb00c1e05 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 09:55:13 +0900 Subject: [PATCH 7/9] reverted debugging line Signed-off-by: kminoda --- localization/ndt_scan_matcher/src/map_module.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp index 36ac18ef77e16..d3eb77797eefd 100644 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -34,7 +34,6 @@ void MapModule::callback_map_points( { NormalDistributionsTransform new_ndt; new_ndt.setParams(ndt_ptr_->getParams()); - new_ndt.setNumThreads(2); pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); From 179f692146960de91223f6c20059c405cc0d7811 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 10:32:58 +0900 Subject: [PATCH 8/9] pre-commit Signed-off-by: kminoda --- .../ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp | 2 ++ 1 file changed, 2 insertions(+) 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 9c3db78b22d92..a38877038df85 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 @@ -24,6 +24,8 @@ #include #include +#include + class MapModule { using PointSource = pcl::PointXYZ; From d7e4c7a6de24c68113e0c7b5a8d8b82601c29373 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 13:26:46 +0900 Subject: [PATCH 9/9] remove format inclusion Signed-off-by: kminoda --- .../ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp | 1 - 1 file changed, 1 deletion(-) 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 a38877038df85..871ff2760acf7 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,7 +19,6 @@ #include -#include #include #include #include