Skip to content

Commit 9161649

Browse files
Fixed update_ndt
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
1 parent 145734c commit 9161649

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
@@ -86,7 +86,11 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
8686
// the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
8787
// If the updating is done the main ndt_ptr_, either the update or the NDT
8888
// align will be blocked by the other.
89-
update_ndt(position, *secondary_ndt_ptr_);
89+
const bool updated = update_ndt(position, *secondary_ndt_ptr_);
90+
if (!updated) {
91+
last_update_position_ = position;
92+
return;
93+
}
9094

9195
ndt_ptr_mutex_->lock();
9296
auto dummy_ptr = ndt_ptr_;
@@ -108,7 +112,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
108112
publish_partial_pcd_map();
109113
}
110114

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

@@ -130,7 +134,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
130134
while (status != std::future_status::ready) {
131135
RCLCPP_INFO(logger_, "waiting response");
132136
if (!rclcpp::ok()) {
133-
return;
137+
return false; // No update
134138
}
135139
status = result.wait_for(std::chrono::seconds(1));
136140
}
@@ -142,7 +146,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
142146
logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size());
143147
if (maps_to_add.empty() && map_ids_to_remove.empty()) {
144148
RCLCPP_INFO(logger_, "Skip map update");
145-
return;
149+
return false; // No update
146150
}
147151

148152
const auto exe_start_time = std::chrono::system_clock::now();
@@ -172,6 +176,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
172176
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
173177
const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
174178
RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
179+
return true; // Updated
175180
}
176181

177182
void MapUpdateModule::publish_partial_pcd_map()

0 commit comments

Comments
 (0)