Permalink
Browse files

coherent handling of treat_unknown_as_occupied

  • Loading branch information...
SebastianInd committed Mar 23, 2018
1 parent 22accea commit 1cd9d0a16e0e0a8c24c8ae86d74ccdf92a8b7b9b
Showing with 43 additions and 6 deletions.
  1. +5 −1 octomap_world/include/octomap_world/octomap_world.h
  2. +38 −5 octomap_world/src/octomap_world.cc
@@ -153,11 +153,15 @@ class OctomapWorld : public WorldBase {
const BoundHandling& insertion_method = BoundHandling::kDefault);
virtual void setBordersOccupied(const Eigen::Vector3d& cropping_size);
// Virtual functions for outputting map status.
// Virtual functions for outputting map status. If treat_unknown_as_occupied
// is set to true, those functions return kOccupied instead of kUnknown.
void enableTreatUnknownAsOccupied();
void disableTreatUnknownAsOccupied();
virtual CellStatus getCellStatusBoundingBox(
const Eigen::Vector3d& point,
const Eigen::Vector3d& bounding_box_size) const;
virtual CellStatus getCellStatusPoint(const Eigen::Vector3d& point) const;
virtual CellStatus getCellTrueStatusPoint(const Eigen::Vector3d& point) const;
virtual CellStatus getCellProbabilityPoint(const Eigen::Vector3d& point,
double* probability) const;
virtual CellStatus getLineStatus(const Eigen::Vector3d& start,
@@ -263,22 +263,30 @@ void OctomapWorld::updateOccupancy(octomap::KeySet* free_cells,
octree_->updateInnerOccupancy();
}
void OctomapWorld::enableTreatUnknownAsOccupied() {
params_.treat_unknown_as_occupied = true;
}
void OctomapWorld::disableTreatUnknownAsOccupied() {
params_.treat_unknown_as_occupied = false;
}
OctomapWorld::CellStatus OctomapWorld::getCellStatusBoundingBox(
const Eigen::Vector3d& point,
const Eigen::Vector3d& bounding_box_size) const {
// First case: center point is unknown or occupied. Can just quit.
CellStatus center_status = getCellStatusPoint(point);
if (center_status != CellStatus::kFree && params_.treat_unknown_as_occupied) {
if (center_status != CellStatus::kFree) {
return center_status;
}
// Also if center is outside of the bounds.
octomap::OcTreeKey key;
if (!octree_->coordToKeyChecked(pointEigenToOctomap(point), key)) {
if (params_.treat_unknown_as_occupied) {
return CellStatus::kUnknown;
} else {
return CellStatus::kOccupied;
} else {
return CellStatus::kUnknown;
}
}
@@ -326,14 +334,35 @@ OctomapWorld::CellStatus OctomapWorld::getCellStatusBoundingBox(
octomap::point3d_list unknown_centers;
octree_->getUnknownLeafCenters(unknown_centers, bbx_min, bbx_max);
if (unknown_centers.size() > 0) {
return CellStatus::kUnknown;
if (params_.treat_unknown_as_occupied) {
return CellStatus::kOccupied;
} else {
return CellStatus::kUnknown;
}
}
return CellStatus::kFree;
}
OctomapWorld::CellStatus OctomapWorld::getCellStatusPoint(
const Eigen::Vector3d& point) const {
octomap::OcTreeNode* node = octree_->search(point.x(), point.y(), point.z());
if (node == NULL) {
if (params_.treat_unknown_as_occupied) {
return CellStatus::kOccupied;
} else {
return CellStatus::kUnknown;
}
} else if (octree_->isNodeOccupied(node)) {
return CellStatus::kOccupied;
} else {
return CellStatus::kFree;
}
}
// Returns kUnknown even if treat_unknown_as_occupied is true.
OctomapWorld::CellStatus OctomapWorld::getCellTrueStatusPoint(
const Eigen::Vector3d& point) const {
octomap::OcTreeNode* node = octree_->search(point.x(), point.y(), point.z());
if (node == NULL) {
return CellStatus::kUnknown;
} else if (octree_->isNodeOccupied(node)) {
@@ -376,7 +405,11 @@ OctomapWorld::CellStatus OctomapWorld::getLineStatus(
for (octomap::OcTreeKey key : key_ray) {
octomap::OcTreeNode* node = octree_->search(key);
if (node == NULL) {
return CellStatus::kUnknown;
if (params_.treat_unknown_as_occupied) {
return CellStatus::kOccupied;
} else {
return CellStatus::kUnknown;
}
} else if (octree_->isNodeOccupied(node)) {
return CellStatus::kOccupied;
}

0 comments on commit 1cd9d0a

Please sign in to comment.