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 f0a132a56cb3a..1ea5ca97134f0 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 @@ -294,7 +294,8 @@ class NDTScanMatcher : public rclcpp::Node bool ndt_service_align_in_progress_{false}; - + rclcpp::CallbackGroup::SharedPtr map_callback_group_; + }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ 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 951e5d28e5878..7ead151afebb9 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -181,6 +181,7 @@ NDTScanMatcher::NDTScanMatcher() ndt_ptr_->setRegularizationScaleFactor(regularization_scale_factor_); copyNDT(ndt_ptr_, backup_ndt_ptr_, ndt_implement_type_); + RCLCPP_INFO( get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", @@ -226,8 +227,8 @@ NDTScanMatcher::NDTScanMatcher() rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::CallbackGroup::SharedPtr map_callback_group; - map_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // rclcpp::CallbackGroup::SharedPtr map_callback_group_; + map_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto initial_pose_sub_opt = rclcpp::SubscriptionOptions(); initial_pose_sub_opt.callback_group = initial_pose_callback_group; @@ -236,7 +237,7 @@ NDTScanMatcher::NDTScanMatcher() main_sub_opt.callback_group = main_callback_group; auto map_sub_opt = rclcpp::SubscriptionOptions(); - map_sub_opt.callback_group = map_callback_group; + map_sub_opt.callback_group = map_callback_group_; initial_pose_sub_ = this->create_subscription( "ekf_pose_with_covariance", 100, @@ -299,7 +300,7 @@ NDTScanMatcher::NDTScanMatcher() service_ = this->create_service( "ndt_align_srv", std::bind(&NDTScanMatcher::serviceNDTAlign, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group); + rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_); // rclcpp::ServicesQoS().get_rmw_qos_profile()); pcd_loader_client_ = this->create_client( @@ -316,7 +317,7 @@ NDTScanMatcher::NDTScanMatcher() auto period_ns = std::chrono::duration_cast(std::chrono::duration(map_update_dt)); map_update_timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&NDTScanMatcher::mapUpdateTimerCallback, this), map_callback_group); + this, get_clock(), period_ns, std::bind(&NDTScanMatcher::mapUpdateTimerCallback, this), map_callback_group_); diagnostic_thread_ = std::thread(&NDTScanMatcher::timerDiagnostic, this); diagnostic_thread_.detach(); diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index ee41c6febf805..aa9d78554d4f4 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -90,21 +90,26 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1)); sub_vector_map_ = this->create_subscription( "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1)); + // publish(); + } void ElevationMapLoaderNode::publish() { - struct stat info; - if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) { - RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map "); - createElevationMap(); - } else if (info.st_mode & S_IFDIR) { + // struct stat info; + // if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) { + // RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map "); + // createElevationMap(); + // } else if (info.st_mode & S_IFDIR) { + data_manager_.elevation_map_path_ = std::make_unique( + std::filesystem::path(elevation_map_directory_) / "elevation_map_eneos/elevation_map_eneos_0.db3" + ); RCLCPP_INFO( this->get_logger(), "Load elevation map from: %s", data_manager_.elevation_map_path_->c_str()); grid_map::GridMapRosConverter::loadFromBag( *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); - } + // } elevation_map_.setFrameId(map_frame_); auto msg = grid_map::GridMapRosConverter::toMessage(elevation_map_); @@ -126,9 +131,9 @@ void ElevationMapLoaderNode::onMapHash( const auto elevation_map_hash = map_hash->pcd; data_manager_.elevation_map_path_ = std::make_unique( std::filesystem::path(elevation_map_directory_) / elevation_map_hash); - if (data_manager_.isInitialized()) { + // if (data_manager_.isInitialized()) { publish(); - } + // } } void ElevationMapLoaderNode::onPointcloudMap(