void CallbackQueue::callAvailable(ros::WallDuration timeout)
{
setupTLS();
TLS* tls = tls_.get();
{
boost::mutex::scoped_lock lock(mutex_);
if (!enabled_)
{
return;
}
if (callbacks_.empty())
{
if (!timeout.isZero())
{
condition_.timed_wait(lock, boost::posix_time::microseconds(timeout.toSec() * 1000000.0f));
}
if (callbacks_.empty() || !enabled_)
{
return;
}
}
bool was_empty = tls->callbacks.empty();
tls->callbacks.insert(tls->callbacks.end(), callbacks_.begin(), callbacks_.end());
callbacks_.clear();
calling_ += tls->callbacks.size();
if (was_empty)
{
tls->cb_it = tls->callbacks.begin();
}
}
size_t called = 0;
while (!tls->callbacks.empty())
{
if (callOneCB(tls) != Empty)
{
++called;
}
}
{
boost::mutex::scoped_lock lock(mutex_);
calling_ -= called;
}
}
bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances, const ros::Time& timestamp, const Eigen::Affine3d& transformationSensorToMap)
{
if (pointCloud->size() != pointCloudVariances.size()) {
ROS_ERROR("ElevationMap::add: Size of point cloud (%i) and variances (%i) do not agree.",
(int) pointCloud->size(), (int) pointCloudVariances.size());
return false;
}
// Initialization for time calculation.
const ros::WallTime methodStartTime(ros::WallTime::now());
boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
// Update initial time if it is not initialized.
if (initialTime_.toSec() == 0) {
initialTime_ = timestamp;
}
const double scanTimeSinceInitialization = (timestamp - initialTime_).toSec();
for (unsigned int i = 0; i < pointCloud->size(); ++i) {
auto& point = pointCloud->points[i];
Index index;
Position position(point.x, point.y);
if (!rawMap_.getIndex(position, index)) continue; // Skip this point if it does not lie within the elevation map.
auto& elevation = rawMap_.at("elevation", index);
auto& variance = rawMap_.at("variance", index);
auto& horizontalVarianceX = rawMap_.at("horizontal_variance_x", index);
auto& horizontalVarianceY = rawMap_.at("horizontal_variance_y", index);
auto& horizontalVarianceXY = rawMap_.at("horizontal_variance_xy", index);
auto& color = rawMap_.at("color", index);
auto& time = rawMap_.at("time", index);
auto& lowestScanPoint = rawMap_.at("lowest_scan_point", index);
auto& sensorXatLowestScan = rawMap_.at("sensor_x_at_lowest_scan", index);
auto& sensorYatLowestScan = rawMap_.at("sensor_y_at_lowest_scan", index);
auto& sensorZatLowestScan = rawMap_.at("sensor_z_at_lowest_scan", index);
const float& pointVariance = pointCloudVariances(i);
const float scanTimeSinceInitialization = (timestamp - initialTime_).toSec();
if (!rawMap_.isValid(index)) {
// No prior information in elevation map, use measurement.
elevation = point.z;
variance = pointVariance;
horizontalVarianceX = minHorizontalVariance_;
horizontalVarianceY = minHorizontalVariance_;
horizontalVarianceXY = 0.0;
colorVectorToValue(point.getRGBVector3i(), color);
continue;
}
// Deal with multiple heights in one cell.
const double mahalanobisDistance = fabs(point.z - elevation) / sqrt(variance);
if (mahalanobisDistance > mahalanobisDistanceThreshold_) {
if (scanTimeSinceInitialization - time <= scanningDuration_ && elevation > point.z) {
// Ignore point if measurement is from the same point cloud (time comparison) and
// if measurement is lower then the elevation in the map.
} else if (scanTimeSinceInitialization - time <= scanningDuration_) {
// If point is higher.
elevation = point.z;
variance = pointVariance;
} else {
variance += multiHeightNoise_;
}
continue;
}
// Store lowest points from scan for visibility checking.
const float pointHeightPlusUncertainty = point.z + 3.0 * sqrt(pointVariance); // 3 sigma.
if (std::isnan(lowestScanPoint) || pointHeightPlusUncertainty < lowestScanPoint){
lowestScanPoint = pointHeightPlusUncertainty;
const Position3 sensorTranslation(transformationSensorToMap.translation());
sensorXatLowestScan = sensorTranslation.x();
sensorYatLowestScan = sensorTranslation.y();
sensorZatLowestScan = sensorTranslation.z();
}
// Fuse measurement with elevation map data.
elevation = (variance * point.z + pointVariance * elevation) / (variance + pointVariance);
variance = (pointVariance * variance) / (pointVariance + variance);
// TODO Add color fusion.
colorVectorToValue(point.getRGBVector3i(), color);
time = scanTimeSinceInitialization;
// Horizontal variances are reset.
horizontalVarianceX = minHorizontalVariance_;
horizontalVarianceY = minHorizontalVariance_;
horizontalVarianceXY = 0.0;
}
clean();
rawMap_.setTimestamp(timestamp.toNSec()); // Point cloud stores time in microseconds.
const ros::WallDuration duration = ros::WallTime::now() - methodStartTime;
ROS_INFO("Raw map has been updated with a new point cloud in %f s.", duration.toSec());
return true;
}
bool ElevationMap::fuse(const grid_map::Index& topLeftIndex, const grid_map::Index& size)
{
ROS_DEBUG("Fusing elevation map...");
// Nothing to do.
if ((size == 0).any()) return false;
// Initializations.
const ros::WallTime methodStartTime(ros::WallTime::now());
boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
// Copy raw elevation map data for safe multi-threading.
boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
auto rawMapCopy = rawMap_;
scopedLockForRawData.unlock();
// More initializations.
const double halfResolution = fusedMap_.getResolution() / 2.0;
const float minimalWeight = std::numeric_limits<float>::epsilon() * (float) 2.0;
// Conservative cell inclusion for ellipse iterator.
const double ellipseExtension = M_SQRT2 * fusedMap_.getResolution();
// Check if there is the need to reset out-dated data.
if (fusedMap_.getTimestamp() != rawMapCopy.getTimestamp()) resetFusedData();
// Align fused map with raw map.
if (rawMapCopy.getPosition() != fusedMap_.getPosition()) fusedMap_.move(rawMapCopy.getPosition());
// For each cell in requested area.
for (SubmapIterator areaIterator(rawMapCopy, topLeftIndex, size); !areaIterator.isPastEnd(); ++areaIterator) {
// Check if fusion for this cell has already been done earlier.
if (fusedMap_.isValid(*areaIterator)) continue;
if (!rawMapCopy.isValid(*areaIterator)) {
// This is an empty cell (hole in the map).
// TODO.
continue;
}
// Get size of error ellipse.
const float& sigmaXsquare = rawMapCopy.at("horizontal_variance_x", *areaIterator);
const float& sigmaYsquare = rawMapCopy.at("horizontal_variance_y", *areaIterator);
const float& sigmaXYsquare = rawMapCopy.at("horizontal_variance_xy", *areaIterator);
Eigen::Matrix2d covarianceMatrix;
covarianceMatrix << sigmaXsquare, sigmaXYsquare, sigmaXYsquare, sigmaYsquare;
// 95.45% confidence ellipse which is 2.486-sigma for 2 dof problem.
// http://www.reid.ai/2012/09/chi-squared-distribution-table-with.html
const double uncertaintyFactor = 2.486; // sqrt(6.18)
Eigen::EigenSolver<Eigen::Matrix2d> solver(covarianceMatrix);
Eigen::Array2d eigenvalues(solver.eigenvalues().real().cwiseAbs());
Eigen::Array2d::Index maxEigenvalueIndex;
eigenvalues.maxCoeff(&maxEigenvalueIndex);
Eigen::Array2d::Index minEigenvalueIndex;
maxEigenvalueIndex == Eigen::Array2d::Index(0) ? minEigenvalueIndex = 1 : minEigenvalueIndex = 0;
const Length ellipseLength = 2.0 * uncertaintyFactor * Length(eigenvalues(maxEigenvalueIndex), eigenvalues(minEigenvalueIndex)).sqrt() + ellipseExtension;
const double ellipseRotation(atan2(solver.eigenvectors().col(maxEigenvalueIndex).real()(1), solver.eigenvectors().col(maxEigenvalueIndex).real()(0)));
// Requested length and position (center) of submap in map.
Position requestedSubmapPosition;
rawMapCopy.getPosition(*areaIterator, requestedSubmapPosition);
EllipseIterator ellipseIterator(rawMapCopy, requestedSubmapPosition, ellipseLength, ellipseRotation);
// Prepare data fusion.
Eigen::ArrayXf means, weights;
const unsigned int maxNumberOfCellsToFuse = ellipseIterator.getSubmapSize().prod();
means.resize(maxNumberOfCellsToFuse);
weights.resize(maxNumberOfCellsToFuse);
WeightedEmpiricalCumulativeDistributionFunction<float> lowerBoundDistribution;
WeightedEmpiricalCumulativeDistributionFunction<float> upperBoundDistribution;
float maxStandardDeviation = sqrt(eigenvalues(maxEigenvalueIndex));
float minStandardDeviation = sqrt(eigenvalues(minEigenvalueIndex));
Eigen::Rotation2Dd rotationMatrix(ellipseRotation);
std::string maxEigenvalueLayer, minEigenvalueLayer;
if (maxEigenvalueIndex == 0) {
maxEigenvalueLayer = "horizontal_variance_x";
minEigenvalueLayer = "horizontal_variance_y";
} else {
maxEigenvalueLayer = "horizontal_variance_y";
minEigenvalueLayer = "horizontal_variance_x";
}
// For each cell in error ellipse.
size_t i = 0;
for (; !ellipseIterator.isPastEnd(); ++ellipseIterator) {
if (!rawMapCopy.isValid(*ellipseIterator)) {
// Empty cell in submap (cannot be center cell because we checked above).
continue;
}
means[i] = rawMapCopy.at("elevation", *ellipseIterator);
// Compute weight from probability.
Position absolutePosition;
rawMapCopy.getPosition(*ellipseIterator, absolutePosition);
Eigen::Vector2d distanceToCenter = (rotationMatrix * (absolutePosition - requestedSubmapPosition)).cwiseAbs();
float probability1 =
cumulativeDistributionFunction(distanceToCenter.x() + halfResolution, 0.0, maxStandardDeviation)
- cumulativeDistributionFunction(distanceToCenter.x() - halfResolution, 0.0, maxStandardDeviation);
float probability2 =
cumulativeDistributionFunction(distanceToCenter.y() + halfResolution, 0.0, minStandardDeviation)
- cumulativeDistributionFunction(distanceToCenter.y() - halfResolution, 0.0, minStandardDeviation);
const float weight = max(minimalWeight, probability1 * probability2);
weights[i] = weight;
const float standardDeviation = sqrt(rawMapCopy.at("variance", *ellipseIterator));
lowerBoundDistribution.add(means[i] - 2.0 * standardDeviation, weight);
upperBoundDistribution.add(means[i] + 2.0 * standardDeviation, weight);
i++;
}
if (i == 0) {
// Nothing to fuse.
fusedMap_.at("elevation", *areaIterator) = rawMapCopy.at("elevation", *areaIterator);
fusedMap_.at("lower_bound", *areaIterator) = rawMapCopy.at("elevation", *areaIterator) - 2.0 * sqrt(rawMapCopy.at("variance", *areaIterator));
fusedMap_.at("upper_bound", *areaIterator) = rawMapCopy.at("elevation", *areaIterator) + 2.0 * sqrt(rawMapCopy.at("variance", *areaIterator));
fusedMap_.at("color", *areaIterator) = rawMapCopy.at("color", *areaIterator);
continue;
}
// Fuse.
means.conservativeResize(i);
weights.conservativeResize(i);
float mean = (weights * means).sum() / weights.sum();
if (!std::isfinite(mean)) {
ROS_ERROR("Something went wrong when fusing the map: Mean = %f", mean);
continue;
}
// Add to fused map.
fusedMap_.at("elevation", *areaIterator) = mean;
lowerBoundDistribution.compute();
upperBoundDistribution.compute();
fusedMap_.at("lower_bound", *areaIterator) = lowerBoundDistribution.quantile(0.01); // TODO
fusedMap_.at("upper_bound", *areaIterator) = upperBoundDistribution.quantile(0.99); // TODO
// TODO Add fusion of colors.
fusedMap_.at("color", *areaIterator) = rawMapCopy.at("color", *areaIterator);
}
fusedMap_.setTimestamp(rawMapCopy.getTimestamp());
const ros::WallDuration duration(ros::WallTime::now() - methodStartTime);
ROS_INFO("Elevation map has been fused in %f s.", duration.toSec());
return true;
}
CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
{
setupTLS();
TLS* tls = tls_.get();
CallbackInfo cb_info;
{
boost::mutex::scoped_lock lock(mutex_);
if (!enabled_)
{
return Disabled;
}
if (callbacks_.empty())
{
if (!timeout.isZero())
{
condition_.timed_wait(lock, boost::posix_time::microseconds(timeout.toSec() * 1000000.0f));
}
if (callbacks_.empty())
{
return Empty;
}
if (!enabled_)
{
return Disabled;
}
}
D_CallbackInfo::iterator it = callbacks_.begin();
for (; it != callbacks_.end();)
{
CallbackInfo& info = *it;
if (info.marked_for_removal)
{
it = callbacks_.erase(it);
continue;
}
if (info.callback->ready())
{
cb_info = info;
it = callbacks_.erase(it);
break;
}
++it;
}
if (!cb_info.callback)
{
return TryAgain;
}
++calling_;
}
bool was_empty = tls->callbacks.empty();
tls->callbacks.push_back(cb_info);
if (was_empty)
{
tls->cb_it = tls->callbacks.begin();
}
CallOneResult res = callOneCB(tls);
if (res != Empty)
{
boost::mutex::scoped_lock lock(mutex_);
--calling_;
}
return res;
}
bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master)
{
ros::WallTime start_time = ros::WallTime::now();
std::string master_host = getHost();
uint32_t master_port = getPort();
XmlRpc::XmlRpcClient *c = XMLRPCManager::instance()->getXMLRPCClient(master_host, master_port, "/");
bool printed = false;
bool slept = false;
bool ok = true;
do
{
bool b = false;
{
#if defined(__APPLE__)
boost::mutex::scoped_lock lock(g_xmlrpc_call_mutex);
#endif
b = c->execute(method.c_str(), request, response);
}
ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
if (!b && ok)
{
if (!printed && wait_for_master)
{
ROS_ERROR("[%s] Failed to contact master at [%s:%d]. %s", method.c_str(), master_host.c_str(), master_port, wait_for_master ? "Retrying..." : "");
printed = true;
}
if (!wait_for_master)
{
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return false;
}
if (!g_retry_timeout.isZero() && (ros::WallTime::now() - start_time) >= g_retry_timeout)
{
ROS_ERROR("[%s] Timed out trying to connect to the master after [%f] seconds", method.c_str(), g_retry_timeout.toSec());
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return false;
}
ros::WallDuration(0.05).sleep();
slept = true;
}
else
{
if (!XMLRPCManager::instance()->validateXmlrpcResponse(method, response, payload))
{
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return false;
}
break;
}
ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
} while(ok);
if (ok && slept)
{
ROS_INFO("Connected to master at [%s:%d]", master_host.c_str(), master_port);
}
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return true;
}