$search

gazebo_ros_openni_kinect.cpp

Go to the documentation of this file.

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 @mainpage
   Desc: GazeboRosOpenniKinect plugin for simulating cameras in Gazebo
   Author: John Hsu and Melonee Wise
   Date: 24 Sept 2008
   SVN info: $Id$
 @htmlinclude manifest.html
 @b GazeboRosOpenniKinect plugin broadcasts ROS Image messages
 */

#include <algorithm>
#include <assert.h>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <gazebo_plugins/gazebo_ros_openni_kinect.h>

#include <gazebo/Timer.hh>
#include <gazebo/Sensor.hh>
#include <gazebo/Model.hh>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <gazebo/MonoCameraSensor.hh>
#include <gazebo/Pose3d.hh>


#include "sensor_msgs/Image.h"
#include "sensor_msgs/fill_image.h"
#include "image_transport/image_transport.h"

#include <geometry_msgs/Point32.h>
#include <sensor_msgs/ChannelFloat32.h>

namespace gazebo
{

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_openni_kinect", GazeboRosOpenniKinect);

// Constructor
GazeboRosOpenniKinect::GazeboRosOpenniKinect(Entity *parent)
    : Controller(parent)
{
  this->myParent = dynamic_cast<MonoCameraSensor*>(this->parent);

  if (!this->myParent)
    gzthrow("GazeboRosOpenniKinect controller requires a Camera Sensor as its parent");

  Param::Begin(&this->parameters);
  this->robotNamespaceP = new ParamT<std::string>("robotNamespace","/",0);
  this->imageTopicNameP = new ParamT<std::string>("imageTopicName","image_raw", 0);
  this->cameraInfoTopicNameP = new ParamT<std::string>("cameraInfoTopicName","camera_info", 0);
  this->depthImageCameraInfoTopicNameP = new ParamT<std::string>("depthImageCameraInfoTopicName","depth/camera_info", 0);
  this->pointCloudTopicNameP = new ParamT<std::string>("pointCloudTopicName","points", 0);
  this->depthImageTopicNameP = new ParamT<std::string>("depthImageTopicName","depth/image_raw", 0);
  this->cameraNameP = new ParamT<std::string>("cameraName","", 0);
  this->frameNameP = new ParamT<std::string>("frameName","generic_camera_link", 0);
  // camera parameters 
  this->CxPrimeP = new ParamT<double>("CxPrime",0, 0); // default to 0 for compute on the fly
  this->CxP  = new ParamT<double>("Cx" ,0, 0); // default to 0 for compute on the fly
  this->CyP  = new ParamT<double>("Cy" ,0, 0); // default to 0 for compute on the fly
  this->focal_lengthP  = new ParamT<double>("focal_length" ,0, 0); // == image_width(px) / (2*tan( hfov(radian) /2)), default to 0 for compute on the fly
  this->hack_baselineP  = new ParamT<double>("hackBaseline" ,0, 0); // hack for right stereo camera
  this->pointCloudCutoffP  = new ParamT<double>("pointCloudCutoff" ,0.4, 0);
  this->distortion_k1P  = new ParamT<double>("distortion_k1" ,0, 0);
  this->distortion_k2P  = new ParamT<double>("distortion_k2" ,0, 0);
  this->distortion_k3P  = new ParamT<double>("distortion_k3" ,0, 0);
  this->distortion_t1P  = new ParamT<double>("distortion_t1" ,0, 0);
  this->distortion_t2P  = new ParamT<double>("distortion_t2" ,0, 0);
  Param::End();

  this->imageConnectCount = 0;
  this->infoConnectCount = 0;
  this->depthInfoConnectCount = 0;
  this->pointCloudConnectCount = 0;
  this->depthImageConnectCount = 0;

  last_image_pub_time_       = Time(0);
  last_camera_info_pub_time_ = Time(0);
  last_point_cloud_pub_time_ = Time(0);

  // set sensor update rate to controller update rate?
  //(dynamic_cast<OgreCamera*>(this->myParent))->SetUpdateRate(this->updateRateP->GetValue());
}

void GazeboRosOpenniKinect::configCallback(gazebo_plugins::GazeboRosOpenniKinectConfig &config, uint32_t level)
{
  ROS_INFO("Reconfigure request for the gazebo ros camera: %s. New rate: %.2f", this->cameraName.c_str(), config.imager_rate);

  (dynamic_cast<OgreCamera*>(this->myParent))->SetUpdateRate(config.imager_rate);
}

// Destructor
GazeboRosOpenniKinect::~GazeboRosOpenniKinect()
{
  delete this->robotNamespaceP;
  delete this->rosnode_;
  delete this->managernode_;
  delete this->manager_;
  delete this->imageTopicNameP;
  delete this->cameraInfoTopicNameP;
  delete this->depthImageCameraInfoTopicNameP;
  delete this->pointCloudTopicNameP;
  delete this->depthImageTopicNameP;
  delete this->frameNameP;
  delete this->CxPrimeP;
  delete this->CxP;
  delete this->CyP;
  delete this->focal_lengthP;
  delete this->hack_baselineP;
  delete this->pointCloudCutoffP;
  delete this->distortion_k1P;
  delete this->distortion_k2P;
  delete this->distortion_k3P;
  delete this->distortion_t1P;
  delete this->distortion_t2P;
}

// Load the controller
void GazeboRosOpenniKinect::LoadChild(XMLConfigNode *node)
{
  this->robotNamespaceP->Load(node);
  this->robotNamespace = this->robotNamespaceP->GetValue();
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  this->imageTopicNameP->Load(node);
  this->cameraInfoTopicNameP->Load(node);
  this->depthImageCameraInfoTopicNameP->Load(node);
  this->pointCloudTopicNameP->Load(node);
  this->depthImageTopicNameP->Load(node);
  this->cameraNameP->Load(node);
  this->frameNameP->Load(node);
  this->CxPrimeP->Load(node);
  this->CxP->Load(node);
  this->CyP->Load(node);
  this->focal_lengthP->Load(node);
  this->hack_baselineP->Load(node);
  this->pointCloudCutoffP->Load(node);
  this->distortion_k1P->Load(node);
  this->distortion_k2P->Load(node);
  this->distortion_k3P->Load(node);
  this->distortion_t1P->Load(node);
  this->distortion_t2P->Load(node);
  this->imageTopicName = this->imageTopicNameP->GetValue();
  this->cameraInfoTopicName = this->cameraInfoTopicNameP->GetValue();
  this->depthImageCameraInfoTopicName = this->depthImageCameraInfoTopicNameP->GetValue();
  this->pointCloudTopicName = this->pointCloudTopicNameP->GetValue();
  this->depthImageTopicName = this->depthImageTopicNameP->GetValue();
  this->frameName = this->frameNameP->GetValue();
  this->CxPrime = this->CxPrimeP->GetValue();
  this->Cx = this->CxP->GetValue();
  this->Cy = this->CyP->GetValue();
  this->focal_length = this->focal_lengthP->GetValue();
  this->hack_baseline = this->hack_baselineP->GetValue();
  this->pointCloudCutoff = this->pointCloudCutoffP->GetValue();
  this->distortion_k1 = this->distortion_k1P->GetValue();
  this->distortion_k2 = this->distortion_k2P->GetValue();
  this->distortion_k3 = this->distortion_k3P->GetValue();
  this->distortion_t1 = this->distortion_t1P->GetValue();
  this->distortion_t2 = this->distortion_t2P->GetValue();
  if ((this->distortion_k1 != 0.0) || (this->distortion_k2 != 0.0) ||
      (this->distortion_k3 != 0.0) || (this->distortion_t1 != 0.0) ||
      (this->distortion_t2 != 0.0))
    ROS_WARN("gazebo_ros_camera simulation does not support non-zero distortion parameters right now, your simulation maybe wrong.");

  this->cameraName = this->cameraNameP->GetValue();
  this->rosnode_ = new ros::NodeHandle(this->robotNamespace+"/"+this->cameraName);
  if (!this->cameraName.empty())
    this->managernode_ = new ros::NodeHandle(this->robotNamespace+"/"+this->cameraName+"_manager");
  else
    this->managernode_ = new ros::NodeHandle(this->robotNamespace+"/"+this->GetName()+"_manager");
  this->manager_ = new nodelet::Loader(*this->managernode_);
  this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);

  if (!this->cameraName.empty()) {
    dyn_srv_ = new dynamic_reconfigure::Server<gazebo_plugins::GazeboRosOpenniKinectConfig>(*this->rosnode_);
    dynamic_reconfigure::Server<gazebo_plugins::GazeboRosOpenniKinectConfig>::CallbackType f = boost::bind(&GazeboRosOpenniKinect::configCallback, this, _1, _2);
    dyn_srv_->setCallback(f);
  }


  this->image_pub_ = this->itnode_->advertise(
    this->imageTopicName,1,
    boost::bind( &GazeboRosOpenniKinect::ImageConnect,this),
    boost::bind( &GazeboRosOpenniKinect::ImageDisconnect,this), ros::VoidPtr(), &this->camera_queue_);

  ros::AdvertiseOptions camera_info_ao = ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
    this->cameraInfoTopicName,1,
    boost::bind( &GazeboRosOpenniKinect::InfoConnect,this),
    boost::bind( &GazeboRosOpenniKinect::InfoDisconnect,this), ros::VoidPtr(), &this->camera_queue_);
  this->camera_info_pub_ = this->rosnode_->advertise(camera_info_ao);

  ros::SubscribeOptions zoom_so = ros::SubscribeOptions::create<std_msgs::Float64>(
    "set_hfov",1,
    boost::bind( &GazeboRosOpenniKinect::SetHFOV,this,_1),
    ros::VoidPtr(), &this->camera_queue_);
  this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so);

  ros::SubscribeOptions rate_so = ros::SubscribeOptions::create<std_msgs::Float64>(
    "set_update_rate",1,
    boost::bind( &GazeboRosOpenniKinect::SetUpdateRate,this,_1),
    ros::VoidPtr(), &this->camera_queue_);
  this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so);

  ros::AdvertiseOptions point_cloud_ao = ros::AdvertiseOptions::create<pcl::PointCloud<pcl::PointXYZ> >(
    this->pointCloudTopicName,1,
    boost::bind( &GazeboRosOpenniKinect::PointCloudConnect,this),
    boost::bind( &GazeboRosOpenniKinect::PointCloudDisconnect,this), ros::VoidPtr(), &this->camera_queue_);
  this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao);

  ros::AdvertiseOptions depth_image_ao = 
    ros::AdvertiseOptions::create< sensor_msgs::Image >(
      this->depthImageTopicName,1,
      boost::bind( &GazeboRosOpenniKinect::DepthImageConnect,this),
      boost::bind( &GazeboRosOpenniKinect::DepthImageDisconnect,this), 
      ros::VoidPtr(), &this->camera_queue_);
  this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao);

  ros::AdvertiseOptions depth_image_camera_info_ao = 
    ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
        this->depthImageCameraInfoTopicName,1,
        boost::bind( &GazeboRosOpenniKinect::DepthInfoConnect,this),
        boost::bind( &GazeboRosOpenniKinect::DepthInfoDisconnect,this), 
        ros::VoidPtr(), &this->camera_queue_);
  this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao);

}

// Increment count
void GazeboRosOpenniKinect::InfoConnect()
{
  this->infoConnectCount++;
}
// Decrement count
void GazeboRosOpenniKinect::InfoDisconnect()
{
  this->infoConnectCount--;
}

// Increment count
void GazeboRosOpenniKinect::DepthInfoConnect()
{
  this->depthInfoConnectCount++;
}
// Decrement count
void GazeboRosOpenniKinect::DepthInfoDisconnect()
{
  this->depthInfoConnectCount--;
}

// Set Horizontal Field of View
void GazeboRosOpenniKinect::SetHFOV(const std_msgs::Float64::ConstPtr& hfov)
{
  (dynamic_cast<OgreCamera*>(this->myParent))->SetFOV(hfov->data);
}

// Set Update Rate
void GazeboRosOpenniKinect::SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate)
{
  (dynamic_cast<OgreCamera*>(this->myParent))->SetUpdateRate(update_rate->data);
}

// Increment count
void GazeboRosOpenniKinect::ImageConnect()
{
  this->imageConnectCount++;
  this->myParent->SetActive(true);
}
// Decrement count
void GazeboRosOpenniKinect::ImageDisconnect()
{
  this->imageConnectCount--;

  if (this->pointCloudConnectCount == 0)
  {
    this->myParent->SimulateDepthData(false);
    if (this->imageConnectCount == 0)
      this->myParent->SetActive(false);
  }
}

// Increment count
void GazeboRosOpenniKinect::PointCloudConnect()
{
  this->pointCloudConnectCount++;
  this->myParent->SetActive(true);
  this->myParent->SimulateDepthData(true);
}
// Decrement count
void GazeboRosOpenniKinect::PointCloudDisconnect()
{
  this->pointCloudConnectCount--;

  if (this->pointCloudConnectCount == 0)
  {
    this->myParent->SimulateDepthData(false);
    if (this->imageConnectCount == 0)
      this->myParent->SetActive(false);
  }
}

// Increment count
void GazeboRosOpenniKinect::DepthImageConnect()
{
  this->depthImageConnectCount++;
  this->myParent->SetActive(true);
  this->myParent->SimulateDepthData(true);
}
// Decrement count
void GazeboRosOpenniKinect::DepthImageDisconnect()
{
  this->depthImageConnectCount--;

  if (this->depthImageConnectCount == 0)
  {
    this->myParent->SimulateDepthData(false);
    if (this->imageConnectCount == 0)
      this->myParent->SetActive(false);
  }
}

// Initialize the controller
void GazeboRosOpenniKinect::InitChild()
{
  // sensor generation off by default
  this->myParent->SetActive(false);
  this->myParent->SimulateDepthData(false);

  // set buffer size
  this->width            = this->myParent->GetImageWidth();
  this->height           = this->myParent->GetImageHeight();
  this->depth            = this->myParent->GetImageDepth();
  if (this->myParent->GetImageFormat() == "L8")
  {
    this->type = sensor_msgs::image_encodings::MONO8;
    this->skip = 1;
  }
  else if (this->myParent->GetImageFormat() == "R8G8B8")
  {
    this->type = sensor_msgs::image_encodings::RGB8;
    this->skip = 3;
  }
  else if (this->myParent->GetImageFormat() == "B8G8R8")
  {
    this->type = sensor_msgs::image_encodings::BGR8;
    this->skip = 3;
  }
  else if (this->myParent->GetImageFormat() == "BAYER_RGGB8")
  {
    ROS_WARN("bayer simulation maybe computationally expensive.");
    this->type = sensor_msgs::image_encodings::BAYER_RGGB8;
    this->skip = 1;
  }
  else if (this->myParent->GetImageFormat() == "BAYER_BGGR8")
  {
    ROS_WARN("bayer simulation maybe computationally expensive.");
    this->type = sensor_msgs::image_encodings::BAYER_BGGR8;
    this->skip = 1;
  }
  else if (this->myParent->GetImageFormat() == "BAYER_GBRG8")
  {
    ROS_WARN("bayer simulation maybe computationally expensive.");
    this->type = sensor_msgs::image_encodings::BAYER_GBRG8;
    this->skip = 1;
  }
  else if (this->myParent->GetImageFormat() == "BAYER_GRBG8")
  {
    ROS_WARN("bayer simulation maybe computationally expensive.");
    this->type = sensor_msgs::image_encodings::BAYER_GRBG8;
    this->skip = 1;
  }
  else
  {
    ROS_ERROR("Unsupported Gazebo ImageFormat\n");
    this->type = sensor_msgs::image_encodings::BGR8;
    this->skip = 3;
  }

  if (this->CxPrime == 0)
    this->CxPrime = ((double)this->width+1.0) /2.0;
  if (this->Cx == 0)
    this->Cx = ((double)this->width+1.0) /2.0;
  if (this->Cy == 0)
    this->Cy = ((double)this->height+1.0) /2.0;


  double computed_focal_length = ((double)this->width) / (2.0 *tan(this->myParent->GetHFOV().GetAsRadian()/2.0));
  if (this->focal_length == 0)
    this->focal_length = computed_focal_length;
  else
    if (fabs(this->focal_length - computed_focal_length) > 1e-8) // check against float precision
      ROS_WARN("The <focal_length>[%f] you have provided for camera [%s] is inconsistent with specified image_width [%d] and HFOV [%f].   Please double check to see that focal_length = width / (2.0 * tan( HFOV/2.0 )), the explected focal_lengtth value is [%f], please update your camera model description accordingly.",
                this->focal_length,this->myParent->GetName().c_str(),this->width,this->myParent->GetHFOV().GetAsRadian(),
                computed_focal_length);


  // start custom queue for camera
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosOpenniKinect::CameraQueueThread,this ) );


}

// Update the controller
void GazeboRosOpenniKinect::UpdateChild()
{

  // Only publish to ROS when the camera updates
  Time sensor_update_time = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime();

  // as long as ros is connected, parent is active
  if (this->myParent->IsActive())
  {
    if (sensor_update_time > last_image_pub_time_)
      this->PutCameraData();
  }

  if (this->infoConnectCount > 0) //publish regardless camera image subscription
    this->PublishCameraInfo(this->camera_info_pub_); 

  if (this->depthInfoConnectCount > 0) //publish regardless camera image subscription
    this->PublishCameraInfo(this->depth_image_camera_info_pub_);
}

// Finalize the controller
void GazeboRosOpenniKinect::FiniChild()
{
  this->myParent->SetActive(false);
  this->manager_->clear();
  this->managernode_->shutdown();
  this->rosnode_->shutdown();
  this->camera_queue_.clear();
  this->camera_queue_.disable();
  this->callback_queue_thread_.join();

}

// Put laser data to the interface
void GazeboRosOpenniKinect::PutCameraData()
{
  //ROS_ERROR("debug %d %s",this->imageConnectCount,this->GetName().c_str());
  const unsigned char *src;
  const float* depths;

  // Get a pointer to image data
  if (this->imageConnectCount > 0)
  {
    // prevent parent from updating the image and the depth data while we are retrieving it
    boost::recursive_mutex::scoped_lock mr_lock(*Simulator::Instance()->GetMRMutex());

    //DIAGNOSTICTIMER(timer("gazebo_ros_camera: GetImageData",6));
    src = this->myParent->GetImageData(0);
    if (src)
    {
      //double tmpT0 = Simulator::Instance()->GetWallTime();

      this->lock.lock();
      // copy data into image
      this->imageMsg.header.frame_id = this->frameName;
      Time lastRenderTime = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime();
      //Time lastRenderTime = Simulator::Instance()->GetSimTime();
      //printf("name[%s] render[%f] vs. sim time[%f], diff[%f]\n",this->GetName().c_str(),lastRenderTime.Double(),Simulator::Instance()->GetSimTime().Double(),lastRenderTime.Double()-Simulator::Instance()->GetSimTime().Double());
      //ROS_DEBUG("camera time %f %d %d",lastRenderTime.Double(),lastRenderTime.sec,lastRenderTime.nsec);
      this->imageMsg.header.stamp.sec = lastRenderTime.sec;
      this->imageMsg.header.stamp.nsec = lastRenderTime.nsec;

      //double tmpT1 = Simulator::Instance()->GetWallTime();
      //double tmpT2;

      if (this->image_pub_.getNumSubscribers() > 0)
      {

        // copy from src to imageMsg
        fillImage(this->imageMsg,
                  this->type,
                  this->height,
                  this->width,
                  this->skip*this->width,
                  (void*)src );

        //tmpT2 = Simulator::Instance()->GetWallTime();

        // publish to ros
        last_image_pub_time_ = Simulator::Instance()->GetSimTime();
        this->image_pub_.publish(this->imageMsg);
      }

      //double tmpT3 = Simulator::Instance()->GetWallTime();

      this->lock.unlock();
    }
  }

  if (this->pointCloudConnectCount > 0 || this->depthImageConnectCount > 0)
  {
    // prevent parent from updating the image and the depth data while we are retrieving it
    boost::recursive_mutex::scoped_lock mr_lock(*Simulator::Instance()->GetMRMutex());

    depths = this->myParent->GetDepthData(0);
    if (depths)
    {
      Time lastRenderTime = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime();
      //double tmpT3 = Simulator::Instance()->GetWallTime();
      if (this->pointCloudConnectCount > 0)
      {
        //double tmpT0 = Simulator::Instance()->GetWallTime();
        this->lock.lock();
        this->pointCloudMsg.header.frame_id = this->frameName;
        this->pointCloudMsg.header.stamp.sec = lastRenderTime.sec;
        this->pointCloudMsg.header.stamp.nsec = lastRenderTime.nsec;
        this->pointCloudMsg.width = this->width;
        this->pointCloudMsg.height = this->height;

        FillPointCloudHelper(this->pointCloudMsg,
                  this->height,
                  this->width,
                  this->skip,
                  (void*)depths );

        // publish to ros
        this->point_cloud_pub_.publish(this->pointCloudMsg);
        this->lock.unlock();
      }

      if (this->depthImageConnectCount > 0)
      {
        this->lock.lock();
        // copy data into image
        this->depth_image_msg_.header.frame_id = this->frameName;
        this->depth_image_msg_.header.stamp.sec = lastRenderTime.sec;
        this->depth_image_msg_.header.stamp.nsec = lastRenderTime.nsec;

        FillDepthImageHelper(this->depth_image_msg_,
                       this->height,
                       this->width,
                       this->skip,
                       (void*)depths );

        this->depth_image_pub_.publish(this->depth_image_msg_);
        this->lock.unlock();
      }
    }
  }
}


// fill a depth image according to http://ros.org/wiki/openni_camera
//   i.e. mm in uint16_t
bool GazeboRosOpenniKinect::FillDepthImageHelper(
    sensor_msgs::Image& image_msg,
    uint32_t height, uint32_t width,
    uint32_t step, void* data_arg)
{
  image_msg.encoding = sensor_msgs::image_encodings::MONO8;
  image_msg.height = height;
  image_msg.width  = width;
  image_msg.step   = 1;
  image_msg.data.resize(height*width*sizeof(uint16_t));
  image_msg.is_bigendian = 0;

  uint16_t*  dest = (uint16_t*)(&(image_msg.data[0]));
  float* toCopyFrom = (float*)data_arg;
  int index = 0;

  // convert depth to point cloud
  for (uint32_t j=0; j<height; j++)
  {
    for (uint32_t i=0; i<width; i++)
    {
      double depth = 0;
      for (uint32_t s=0; s<step; s++) // if depth > 1 (e.g. rgb), average
        depth += toCopyFrom[index++];
      depth = depth / (double)step;

      if(depth > this->pointCloudCutoff)
      {
        dest[i+j*width] = (uint16_t)1000.0*depth; // uint16 in mm
      }
      else //point in the unseeable range
      {
        dest[i+j*width] = 0;
      }
    }
  }
  return true;
}

// Convert image format from gazebo sensor provided foramt to desired ros format
void GazeboRosOpenniKinect::convertImageFormat(unsigned char *dst, const unsigned char *src)
{
      // do last minute conversion if Bayer pattern is requested but not provided, go from R8G8B8
      // deprecated in gazebo2 branch, keep for backwards compatibility
      if (this->myParent->GetImageFormat() == "BAYER_RGGB8" && this->depth == 3)
      {
        for (int i=0;i<this->width;i++)
        {
          for (int j=0;j<this->height;j++)
          {
            //
            // RG
            // GB
            //
            // determine position
            if (j%2) // even column
              if (i%2) // even row, red
                dst[i+j*this->width] = src[i*3+j*this->width*3+0];
              else // odd row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
            else // odd column
              if (i%2) // even row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
              else // odd row, blue
                dst[i+j*this->width] = src[i*3+j*this->width*3+2];
          }
        }
        src=dst;
      }
      else if (this->myParent->GetImageFormat() == "BAYER_BGGR8" && this->depth == 3)
      {
        for (int i=0;i<this->width;i++)
        {
          for (int j=0;j<this->height;j++)
          {
            //
            // BG
            // GR
            //
            // determine position
            if (j%2) // even column
              if (i%2) // even row, blue
                dst[i+j*this->width] = src[i*3+j*this->width*3+2];
              else // odd row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
            else // odd column
              if (i%2) // even row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
              else // odd row, red
                dst[i+j*this->width] = src[i*3+j*this->width*3+0];
          }
        }
        src=dst;
      }
      else if (this->myParent->GetImageFormat() == "BAYER_GBRG8" && this->depth == 3)
      {
        for (int i=0;i<this->width;i++)
        {
          for (int j=0;j<this->height;j++)
          {
            //
            // GB
            // RG
            //
            // determine position
            if (j%2) // even column
              if (i%2) // even row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
              else // odd row, blue
                dst[i+j*this->width] = src[i*3+j*this->width*3+2];
            else // odd column
              if (i%2) // even row, red
                dst[i+j*this->width] = src[i*3+j*this->width*3+0];
              else // odd row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
          }
        }
        src=dst;
      }
      else if (this->myParent->GetImageFormat() == "BAYER_GRBG8" && this->depth == 3)
      {
        for (int i=0;i<this->width;i++)
        {
          for (int j=0;j<this->height;j++)
          {
            //
            // GR
            // BG
            //
            // determine position
            if (j%2) // even column
              if (i%2) // even row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
              else // odd row, red
                dst[i+j*this->width] = src[i*3+j*this->width*3+0];
            else // odd column
              if (i%2) // even row, blue
                dst[i+j*this->width] = src[i*3+j*this->width*3+2];
              else // odd row, green
                dst[i+j*this->width] = src[i*3+j*this->width*3+1];
          }
        }
        src=dst;
      }
}

// Fill depth information
bool GazeboRosOpenniKinect::FillPointCloudHelper(pcl::PointCloud<pcl::PointXYZ>& point_cloud,
                             uint32_t rows_arg,
                             uint32_t cols_arg,
                             uint32_t step_arg,
                             void* data_arg)
{
  point_cloud.points.resize(0);
  point_cloud.is_dense = true;
  //point_cloud.channels.resize(1);
  //point_cloud.channels[0].values.resize(0);

  float* toCopyFrom = (float*)data_arg;
  int index = 0;

  double hfov = this->myParent->GetHFOV().GetAsRadian();
  double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));
  //ROS_ERROR("debug hfov: %f fl: %f w: %f h: %f",hfov, fl, (double)cols_arg, (double)rows_arg);
  //ROS_ERROR("debug %f %f", this->myParent->GetFarClip() , this->myParent->GetNearClip());

  // convert depth to point cloud
  for (uint32_t j=0; j<rows_arg; j++)
  {
    double pAngle;
    if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl);
    else            pAngle = 0.0;

    for (uint32_t i=0; i<cols_arg; i++)
    {
      double yAngle;
      if (cols_arg>1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl);
      else            yAngle = 0.0;

      //double depth = (this->myParent->GetFarClip() - this->myParent->GetNearClip()) * ( toCopyFrom[index++] ) + 1.0 * this->myParent->GetNearClip() ;
      //double depth = ( this->myParent->GetFarClip() + this->myParent->GetNearClip() + toCopyFrom[index++] )
      //               * 0.5 ; //this->myParent->GetNearClip() ;
      double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip();

      // in optical frame
      // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
      // to urdf, where the *_optical_frame should have above relative
      // rotation from the physical camera *_frame
      pcl::PointXYZ point;
      point.x      = depth * tan(yAngle);
      point.y      = depth * tan(pAngle);
      if(depth > this->pointCloudCutoff)
      {
        point.z    = depth;
      }
      else //point in the unseeable range
      {
        point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
        point_cloud.is_dense = false;
      }
      point_cloud.points.push_back(point);
    }
  }
  //this->pointCloudMsg.channels[0].values.push_back(1.0);
  return true;
}
// Put laser data to the interface
void GazeboRosOpenniKinect::PublishCameraInfo(ros::Publisher camera_info_publisher)
{
  sensor_msgs::CameraInfo camera_info_msg;
  // fill CameraInfo
  camera_info_msg.header.frame_id = this->frameName;
  Time lastRenderTime = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime();
  //Time lastRenderTime = Simulator::Instance()->GetSimTime();
  camera_info_msg.header.stamp.sec = lastRenderTime.sec;
  camera_info_msg.header.stamp.nsec = lastRenderTime.nsec;
  camera_info_msg.height = this->height;
  camera_info_msg.width  = this->width;
  // distortion
#if ROS_VERSION_MINIMUM(1, 3, 0)
  camera_info_msg.distortion_model = "plumb_bob";
  camera_info_msg.D.resize(5);
#endif
  camera_info_msg.D[0] = this->distortion_k1;
  camera_info_msg.D[1] = this->distortion_k2;
  camera_info_msg.D[2] = this->distortion_k3;
  camera_info_msg.D[3] = this->distortion_t1;
  camera_info_msg.D[4] = this->distortion_t2;
  // original camera matrix
  camera_info_msg.K[0] = this->focal_length;
  camera_info_msg.K[1] = 0.0;
  camera_info_msg.K[2] = this->Cx;
  camera_info_msg.K[3] = 0.0;
  camera_info_msg.K[4] = this->focal_length;
  camera_info_msg.K[5] = this->Cy;
  camera_info_msg.K[6] = 0.0;
  camera_info_msg.K[7] = 0.0;
  camera_info_msg.K[8] = 1.0;
  // rectification
  camera_info_msg.R[0] = 1.0;
  camera_info_msg.R[1] = 0.0;
  camera_info_msg.R[2] = 0.0;
  camera_info_msg.R[3] = 0.0;
  camera_info_msg.R[4] = 1.0;
  camera_info_msg.R[5] = 0.0;
  camera_info_msg.R[6] = 0.0;
  camera_info_msg.R[7] = 0.0;
  camera_info_msg.R[8] = 1.0;
  // camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
  camera_info_msg.P[0] = this->focal_length;
  camera_info_msg.P[1] = 0.0;
  camera_info_msg.P[2] = this->Cx;
  camera_info_msg.P[3] = -this->focal_length * this->hack_baseline;
  camera_info_msg.P[4] = 0.0;
  camera_info_msg.P[5] = this->focal_length;
  camera_info_msg.P[6] = this->Cy;
  camera_info_msg.P[7] = 0.0;
  camera_info_msg.P[8] = 0.0;
  camera_info_msg.P[9] = 0.0;
  camera_info_msg.P[10] = 1.0;
  camera_info_msg.P[11] = 0.0;

  last_camera_info_pub_time_ = Simulator::Instance()->GetSimTime();
  camera_info_publisher.publish(camera_info_msg);
}


// Put laser data to the interface
void GazeboRosOpenniKinect::CameraQueueThread()
{
  static const double timeout = 0.01;

  while (this->rosnode_->ok())
  {
    this->camera_queue_.callAvailable(ros::WallDuration(timeout));
  }
}

}

---

gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Sat Mar 2 13:39:46 2013