@@ -208,19 +208,19 @@ void ClearCostmapService::clearEntirely(const std::vector<std::string> & plugins
208208 if (plugins.empty ()) {
209209 // Default behavior: clear all layers
210210 std::unique_lock<Costmap2D::mutex_t > lock (*(costmap_.getCostmap ()->getMutex ()));
211- RCLCPP_INFO (logger_, " Clearing all layers in costmap: %s" , costmap_.getName ().c_str ());
211+ RCLCPP_INFO (logger_, " Clearing all layers in costmap: %s" , costmap_.getName ().c_str ());
212212 costmap_.resetLayers ();
213213 } else {
214214 // Clear only specified plugins
215215 auto layers = costmap_.getLayeredCostmap ()->getPlugins ();
216-
217216 for (auto & layer : *layers) {
218217 if (shouldClearLayer (layer, plugins)) {
219218 if (layer->isClearable ()) {
220219 RCLCPP_INFO (logger_, " Clearing entire layer: %s" , layer->getName ().c_str ());
221220 auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
222- std::unique_lock<Costmap2D::mutex_t > lock (*(costmap_layer->getMutex ()));
223- costmap_layer->resetMap (0 , 0 , costmap_layer->getSizeInCellsX (), costmap_layer->getSizeInCellsY ());
221+ std::unique_lock<Costmap2D::mutex_t > lock (*(costmap_layer->getMutex ()));
222+ costmap_layer->resetMap (0 , 0 , costmap_layer->getSizeInCellsX (),
223+ costmap_layer->getSizeInCellsY ());
224224 } else {
225225 RCLCPP_WARN (
226226 logger_,
@@ -240,18 +240,18 @@ bool ClearCostmapService::shouldClearLayer(
240240 if (plugins.empty ()) {
241241 return layer->isClearable ();
242242 }
243-
243+
244244 // If specific plugins requested, check if this layer is in the list AND clearable
245- bool is_in_plugin_list = std::find (plugins.begin (), plugins.end (), layer-> getName ()) != plugins. end ();
246-
245+ bool is_in_plugin_list = std::find (plugins.begin (), plugins.end (),
246+ layer-> getName ()) != plugins. end ();
247247 if (is_in_plugin_list && !layer->isClearable ()) {
248248 RCLCPP_WARN (
249249 logger_,
250250 " Plugin '%s' was requested to be cleared but is not clearable. Skipping." ,
251251 layer->getName ().c_str ());
252252 return false ;
253253 }
254-
254+
255255 return is_in_plugin_list && layer->isClearable ();
256256}
257257
0 commit comments