////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image,
unsigned int _width, unsigned int _height, unsigned int _depth,
const std::string &_format)
{
if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
return;
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
if (this->parentSensor->IsActive())
{
if (this->point_cloud_connect_count_ <= 0 &&
this->depth_image_connect_count_ <= 0 &&
(*this->image_connect_count_) <= 0)
{
this->parentSensor->SetActive(false);
}
else
{
if (this->point_cloud_connect_count_ > 0)
this->FillPointdCloud(_image);
if (this->depth_image_connect_count_ > 0)
this->FillDepthImage(_image);
}
}
else
{
if (this->point_cloud_connect_count_ > 0 ||
this->depth_image_connect_count_ <= 0)
// do this first so there's chance for sensor to run 1 frame after activate
this->parentSensor->SetActive(true);
}
PublishCameraInfo();
}
void GazeboNoisyDepth::OnNewDepthFrame(const float *_image, unsigned int _width,
unsigned int _height,
unsigned int _depth,
const std::string &_format) {
if (!this->initialized_ || this->height_ <= 0 || this->width_ <= 0) return;
this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
// check if there are subscribers, if not disable parent, else process images..
if (this->parentSensor->IsActive()) {
if (this->depth_image_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) {
this->parentSensor->SetActive(false);
} else {
if (this->depth_image_connect_count_ > 0) this->FillDepthImage(_image);
}
}
else {
// if parent is disabled, but has subscribers, enable it.
if ((*this->image_connect_count_) > 0){
this->parentSensor->SetActive(true);
}
}
PublishCameraInfo();
}
