@@ -146,7 +146,7 @@ bool ClearCostmapService::clearAroundPose(
146146 y = global_pose.pose .position .y ;
147147
148148 auto layers = costmap_.getLayeredCostmap ()->getPlugins ();
149-
149+
150150 if (!plugins.empty ()) {
151151 return validateAndClearPlugins (
152152 plugins, layers,
@@ -164,7 +164,7 @@ bool ClearCostmapService::clearAroundPose(
164164 any_plugin_cleared = true ;
165165 }
166166 }
167-
167+
168168 return any_plugin_cleared;
169169 }
170170}
@@ -183,7 +183,7 @@ bool ClearCostmapService::clearRegion(
183183 }
184184
185185 auto layers = costmap_.getLayeredCostmap ()->getPlugins ();
186-
186+
187187 if (!plugins.empty ()) {
188188 return validateAndClearPlugins (
189189 plugins, layers,
@@ -201,7 +201,7 @@ bool ClearCostmapService::clearRegion(
201201 any_plugin_cleared = true ;
202202 }
203203 }
204-
204+
205205 return any_plugin_cleared;
206206 }
207207}
@@ -240,22 +240,22 @@ bool ClearCostmapService::clearEntirely(const std::vector<std::string> & plugins
240240 // Clear only specified plugins
241241 std::unique_lock<Costmap2D::mutex_t > lock (*(costmap_.getCostmap ()->getMutex ()));
242242 auto layers = costmap_.getLayeredCostmap ()->getPlugins ();
243-
243+
244244 bool result = validateAndClearPlugins (
245245 plugins, layers,
246246 [this ](std::shared_ptr<CostmapLayer> & layer) {
247247 RCLCPP_INFO (logger_, " Clearing entire layer: %s" , layer->getName ().c_str ());
248248 layer->resetMap (0 , 0 , layer->getSizeInCellsX (), layer->getSizeInCellsY ());
249249 },
250250 " clear costmap entirely" );
251-
251+
252252 if (result) {
253253 RCLCPP_INFO (logger_, " Resetting master costmap after plugin clearing" );
254254 costmap_.getCostmap ()->resetMap (0 , 0 ,
255255 costmap_.getCostmap ()->getSizeInCellsX (),
256256 costmap_.getCostmap ()->getSizeInCellsY ());
257257 }
258-
258+
259259 return result;
260260 }
261261}
@@ -268,19 +268,19 @@ void ClearCostmapService::validateAndCategorizePlugins(
268268{
269269 valid_plugins.clear ();
270270 invalid_plugins.clear ();
271-
271+
272272 for (const auto & requested_plugin : requested_plugins) {
273273 bool found = false ;
274274 bool clearable = false ;
275-
275+
276276 for (auto & layer : *layers) {
277277 if (layer->getName () == requested_plugin) {
278278 found = true ;
279279 clearable = layer->isClearable ();
280280 break ;
281281 }
282282 }
283-
283+
284284 if (!found) {
285285 invalid_plugins.push_back (requested_plugin + " (not found)" );
286286 } else if (!clearable) {
@@ -299,10 +299,10 @@ bool ClearCostmapService::validateAndClearPlugins(
299299{
300300 std::vector<std::string> invalid_plugins;
301301 std::vector<std::string> valid_plugins;
302-
302+
303303 // Validate all requested plugins
304304 validateAndCategorizePlugins (plugins, layers, valid_plugins, invalid_plugins);
305-
305+
306306 // Log validation errors
307307 if (!invalid_plugins.empty ()) {
308308 std::string error_msg = " Invalid plugin(s) requested for clearing: " ;
@@ -314,31 +314,33 @@ bool ClearCostmapService::validateAndClearPlugins(
314314 }
315315 RCLCPP_ERROR (logger_, " %s" , error_msg.c_str ());
316316 }
317-
317+
318318 // Clear all valid plugins using the provided callback
319319 bool any_plugin_cleared = false ;
320320 for (auto & layer : *layers) {
321- if (std::find (valid_plugins.begin (), valid_plugins.end (),
322- layer->getName ()) != valid_plugins.end ()) {
321+ if (std::find (valid_plugins.begin (), valid_plugins.end (),
322+ layer->getName ()) != valid_plugins.end ())
323+ {
323324 auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
324325 clear_callback (costmap_layer);
325326 any_plugin_cleared = true ;
326327 }
327328 }
328-
329+
329330 // Return failure if any requested plugin was invalid
330331 if (!invalid_plugins.empty ()) {
331- RCLCPP_ERROR (logger_,
332+ RCLCPP_ERROR (
333+ logger_,
332334 " Failed to %s: %zu invalid plugin(s) out of %zu requested" ,
333335 operation_name.c_str (), invalid_plugins.size (), plugins.size ());
334336 return false ;
335337 }
336-
338+
337339 if (!any_plugin_cleared) {
338340 RCLCPP_ERROR (logger_, " No plugins were cleared in %s" , operation_name.c_str ());
339341 return false ;
340342 }
341-
343+
342344 return true ;
343345}
344346
0 commit comments