Skip to content

Commit 30cd241

Browse files
fix(ndt_scan_matcher): add a bool return indicating whether ndt has been updated in update_ndt (autowarefoundation#6502)
Fixed update_ndt Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
1 parent e64415b commit 30cd241

File tree

2 files changed

+10
-5
lines changed

2 files changed

+10
-5
lines changed

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ class MapUpdateModule
5555
friend class NDTScanMatcher;
5656

5757
// Update the specified NDT
58-
void update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
58+
bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
5959
void update_map(const geometry_msgs::msg::Point & position);
6060
[[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position);
6161
void publish_partial_pcd_map();

localization/ndt_scan_matcher/src/map_update_module.cpp

+9-4
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,11 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
8888
// the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
8989
// If the updating is done the main ndt_ptr_, either the update or the NDT
9090
// align will be blocked by the other.
91-
update_ndt(position, *secondary_ndt_ptr_);
91+
const bool updated = update_ndt(position, *secondary_ndt_ptr_);
92+
if (!updated) {
93+
last_update_position_ = position;
94+
return;
95+
}
9296

9397
ndt_ptr_mutex_->lock();
9498
auto dummy_ptr = ndt_ptr_;
@@ -110,7 +114,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
110114
publish_partial_pcd_map();
111115
}
112116

113-
void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt)
117+
bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt)
114118
{
115119
auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
116120

@@ -132,7 +136,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
132136
while (status != std::future_status::ready) {
133137
RCLCPP_INFO(logger_, "waiting response");
134138
if (!rclcpp::ok()) {
135-
return;
139+
return false; // No update
136140
}
137141
status = result.wait_for(std::chrono::seconds(1));
138142
}
@@ -144,7 +148,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
144148
logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size());
145149
if (maps_to_add.empty() && map_ids_to_remove.empty()) {
146150
RCLCPP_INFO(logger_, "Skip map update");
147-
return;
151+
return false; // No update
148152
}
149153

150154
const auto exe_start_time = std::chrono::system_clock::now();
@@ -174,6 +178,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
174178
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
175179
const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
176180
RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
181+
return true; // Updated
177182
}
178183

179184
void MapUpdateModule::publish_partial_pcd_map()

0 commit comments

Comments
 (0)