| 421 | } |
| 422 | |
| 423 | void CostmapInterface::ClearCostMap() { |
| 424 | std::vector<Layer *> *plugins = layered_costmap_->GetPlugins(); |
| 425 | tf::Stamped<tf::Pose> pose; |
| 426 | if (GetRobotPose(pose) == false) { |
| 427 | return; |
| 428 | } |
| 429 | double pose_x = pose.getOrigin().x(); |
| 430 | double pose_y = pose.getOrigin().y(); |
| 431 | |
| 432 | for (std::vector<roborts_costmap::Layer *>::iterator plugin_iter = plugins->begin(); |
| 433 | plugin_iter != plugins->end(); |
| 434 | ++plugin_iter) { |
| 435 | roborts_costmap::Layer *plugin = *plugin_iter; |
| 436 | if (plugin->GetName().find("obstacle") != std::string::npos) { |
| 437 | CostmapLayer *costmap_layer_ptr = (CostmapLayer *) plugin; |
| 438 | ClearLayer(costmap_layer_ptr, pose_x, pose_y); |
| 439 | } |
| 440 | |
| 441 | } |
| 442 | } |
| 443 | |
| 444 | void CostmapInterface::ClearLayer(CostmapLayer *costmap_layer_ptr, double pose_x, double pose_y) { |
| 445 | std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_layer_ptr->GetMutex())); |
no test coverage detected