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