| 434 | } |
| 435 | |
| 436 | bool ElevationMapping::getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) |
| 437 | { |
| 438 | grid_map::Position requestedSubmapPosition(request.position_x, request.position_y); |
| 439 | Length requestedSubmapLength(request.length_x, request.length_y); |
| 440 | ROS_DEBUG("Elevation submap request: Position x=%f, y=%f, Length x=%f, y=%f.", requestedSubmapPosition.x(), requestedSubmapPosition.y(), requestedSubmapLength(0), requestedSubmapLength(1)); |
| 441 | boost::recursive_mutex::scoped_lock scopedLock(map_.getFusedDataMutex()); |
| 442 | map_.fuseArea(requestedSubmapPosition, requestedSubmapLength); |
| 443 | |
| 444 | bool isSuccess; |
| 445 | Index index; |
| 446 | GridMap subMap = map_.getFusedGridMap().getSubmap(requestedSubmapPosition, requestedSubmapLength, index, isSuccess); |
| 447 | scopedLock.unlock(); |
| 448 | |
| 449 | if (request.layers.empty()) { |
| 450 | GridMapRosConverter::toMessage(subMap, response.map); |
| 451 | } else { |
| 452 | vector<string> layers; |
| 453 | for (const auto& layer : request.layers) { |
| 454 | layers.push_back(layer); |
| 455 | } |
| 456 | GridMapRosConverter::toMessage(subMap, layers, response.map); |
| 457 | } |
| 458 | |
| 459 | ROS_DEBUG("Elevation submap responded with timestamp %f.", map_.getTimeOfLastFusion().toSec()); |
| 460 | return isSuccess; |
| 461 | } |
| 462 | |
| 463 | bool ElevationMapping::clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) |
| 464 | { |
nothing calls this directly
no test coverage detected