| 118 | } |
| 119 | |
| 120 | void GridMapPclLoader::processGridMapCell(const unsigned int linearGridMapIndex, grid_map::Matrix* gridMapData) { |
| 121 | // Get grid map index from linear index and check if enough points lie within the cell |
| 122 | const grid_map::Index index(grid_map::getIndexFromLinearIndex(linearGridMapIndex, workingGridMap_.getSize())); |
| 123 | |
| 124 | Pointcloud::Ptr pointsInsideCellBorder(new Pointcloud()); |
| 125 | pointsInsideCellBorder = getPointcloudInsideGridMapCellBorder(index); |
| 126 | const bool isTooFewPointsInCell = pointsInsideCellBorder->size() < params_.get().gridMap_.minCloudPointsPerCell_; |
| 127 | if (isTooFewPointsInCell) { |
| 128 | ROS_WARN_STREAM_THROTTLE(10.0, "Less than " << params_.get().gridMap_.minCloudPointsPerCell_ << " points in a cell"); |
| 129 | return; |
| 130 | } |
| 131 | auto& clusterHeights = clusterHeightsWithingGridMapCell_[index(0)][index(1)]; |
| 132 | calculateElevationFromPointsInsideGridMapCell(pointsInsideCellBorder, clusterHeights); |
| 133 | if (clusterHeights.empty()) { |
| 134 | (*gridMapData)(index(0), index(1)) = std::nan("1"); |
| 135 | } else { |
| 136 | (*gridMapData)(index(0), index(1)) = params_.get().clusterExtraction_.useMaxHeightAsCellElevation_ |
| 137 | ? *(std::max_element(clusterHeights.begin(), clusterHeights.end())) |
| 138 | : *(std::min_element(clusterHeights.begin(), clusterHeights.end())); |
| 139 | } |
| 140 | } |
| 141 | |
| 142 | void GridMapPclLoader::calculateElevationFromPointsInsideGridMapCell(Pointcloud::ConstPtr cloud, std::vector<float>& heights) const { |
| 143 | heights.clear(); |
nothing calls this directly
no test coverage detected