// What we want to do here is to look at all the points that are *not* included in the indices list,
// (i.e. these are the outliers, the points not on the floor) and determine which are the closest to the camera
void cloudIndicesModelCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const PointIndicesConstPtr& indices, const pcl::ModelCoefficientsConstPtr& model)
{
boost::mutex::scoped_lock lock (callback_mutex);
NODELET_DEBUG_STREAM("Got cloud with timestamp " << cloud_msg->header.stamp << " + indices with timestamp "
<< indices->header.stamp << " + model with timestamp " << model->header.stamp);
double dt;
if (first)
{
first = false;
dt = 0;
}
else
{
dt = (cloud_msg->header.stamp - prev_cloud_time).toSec();
prev_cloud_time = cloud_msg->header.stamp;
}
if (model->values.size() > 0)
{
// Determine altitude:
kinect_z = reject_outliers(-fabs(model->values[3])) - imu_to_kinect_offset;
calcVelocity(kinect_z, dt, kinect_vz);
// Detect obstacles:
pcl::PointXYZ obstacle_location;
bool obstacle_found = detectObstacle(cloud_msg, indices, model, obstacle_location);
if (obstacle_found)
{
publishObstacleMarker(obstacle_location);
NODELET_DEBUG("Detected obstacle at: [%f, %f, %f]", obstacle_location.x, obstacle_location.y,
obstacle_location.z);
}
publishObstacle(obstacle_found, obstacle_location);
}
else
{
NODELET_WARN("No planar model found -- cannot determine altitude or obstacles");
}
updateMask();
if (not use_backup_estimator_alt)
{
publishOdom();
}
if (debug_throttle_rate > 0)
{
ros::Duration(1 / debug_throttle_rate).sleep();
}
}
double compute_clean_mean(double data[], int size, float nsigma)
{
do {} while (reject_outliers(data, size, nsigma) > 0);
return compute_mean(data, 0,size);
}
