@@ -88,7 +88,11 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
88
88
// the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
89
89
// If the updating is done the main ndt_ptr_, either the update or the NDT
90
90
// 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
+ }
92
96
93
97
ndt_ptr_mutex_->lock ();
94
98
auto dummy_ptr = ndt_ptr_;
@@ -110,7 +114,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
110
114
publish_partial_pcd_map ();
111
115
}
112
116
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)
114
118
{
115
119
auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
116
120
@@ -132,7 +136,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
132
136
while (status != std::future_status::ready) {
133
137
RCLCPP_INFO (logger_, " waiting response" );
134
138
if (!rclcpp::ok ()) {
135
- return ;
139
+ return false ; // No update
136
140
}
137
141
status = result.wait_for (std::chrono::seconds (1 ));
138
142
}
@@ -144,7 +148,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
144
148
logger_, " Update map (Add: %lu, Remove: %lu)" , maps_to_add.size (), map_ids_to_remove.size ());
145
149
if (maps_to_add.empty () && map_ids_to_remove.empty ()) {
146
150
RCLCPP_INFO (logger_, " Skip map update" );
147
- return ;
151
+ return false ; // No update
148
152
}
149
153
150
154
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
174
178
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count ();
175
179
const auto exe_time = static_cast <double >(duration_micro_sec) / 1000.0 ;
176
180
RCLCPP_INFO (logger_, " Time duration for creating new ndt_ptr: %lf [ms]" , exe_time);
181
+ return true ; // Updated
177
182
}
178
183
179
184
void MapUpdateModule::publish_partial_pcd_map ()
0 commit comments