$search

gazebo_ros_stereo_camera.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: GazeboRosStereoCamera plugin for simulating cameras in Gazebo
   Author: John Hsu
   Date: 24 Sept 2008
   SVN info: $Id$
 @htmlinclude manifest.html
 @b GazeboRosStereoCamera plugin broadcasts ROS Image messages
 */

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

#include <gazebo_plugins/gazebo_ros_stereo_camera.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 "sensor_msgs/Image.h"
#include "sensor_msgs/fill_image.h"

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

namespace gazebo
{

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_stereo_camera", GazeboRosStereoCamera);

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

  if (!this->myParent)
    gzthrow("GazeboRosStereoCamera 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","", 1);
  this->pointCloudTopicNameP = new ParamT<std::string>("pointCloudTopicName","", 1);
  this->cameraInfoTopicNameP = new ParamT<std::string>("cameraInfoTopicName","", 1);
  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->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->pointCloudConnectCount = 0;
  this->infoConnectCount = 0;
  
  // set sensor update rate to controller update rate?
  //(dynamic_cast<OgreCamera*>(this->myParent))->SetUpdateRate(this->updateRateP->GetValue());
}

// Destructor
GazeboRosStereoCamera::~GazeboRosStereoCamera()
{
  delete this->robotNamespaceP;
  delete this->rosnode_;
  delete this->imageTopicNameP;
  delete this->pointCloudTopicNameP;
  delete this->cameraInfoTopicNameP;
  delete this->frameNameP;
  delete this->CxPrimeP;
  delete this->CxP;
  delete this->CyP;
  delete this->focal_lengthP;
  delete this->hack_baselineP;
  delete this->distortion_k1P;
  delete this->distortion_k2P;
  delete this->distortion_k3P;
  delete this->distortion_t1P;
  delete this->distortion_t2P;


  
}

// Load the controller
void GazeboRosStereoCamera::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->rosnode_ = new ros::NodeHandle(this->robotNamespace);

  this->imageTopicNameP->Load(node);
  this->pointCloudTopicNameP->Load(node);
  this->cameraInfoTopicNameP->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->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->pointCloudTopicName = this->pointCloudTopicNameP->GetValue();
  this->cameraInfoTopicName = this->cameraInfoTopicNameP->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->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();

#ifdef USE_CBQ
  ros::AdvertiseOptions image_ao = ros::AdvertiseOptions::create<sensor_msgs::Image>(
    this->imageTopicName,1,
    boost::bind( &GazeboRosStereoCamera::ImageConnect,this),
    boost::bind( &GazeboRosStereoCamera::ImageDisconnect,this), ros::VoidPtr(), &this->camera_queue_);
  this->image_pub_ = this->rosnode_->advertise(image_ao);

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

  ros::AdvertiseOptions camera_info_ao = ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
    this->cameraInfoTopicName,1,
    boost::bind( &GazeboRosStereoCamera::InfoConnect,this),
    boost::bind( &GazeboRosStereoCamera::InfoDisconnect,this), ros::VoidPtr(), &this->camera_queue_);
  this->camera_info_pub_ = this->rosnode_->advertise(camera_info_ao);
#else
  this->image_pub_ = this->rosnode_->advertise<sensor_msgs::Image>(this->imageTopicName,1,
    boost::bind( &GazeboRosStereoCamera::ImageConnect, this),
    boost::bind( &GazeboRosStereoCamera::ImageDisconnect, this));
  this->point_cloud_pub_ = this->rosnode_->advertise<sensor_msgs::PointCloud>(this->pointCloudTopicName,1,
    boost::bind( &GazeboRosStereoCamera::PointCloudConnect, this),
    boost::bind( &GazeboRosStereoCamera::PointCloudDisconnect, this));
  this->camera_info_pub_ = this->rosnode_->advertise<sensor_msgs::CameraInfo>(this->cameraInfoTopicName,1,
    boost::bind( &GazeboRosStereoCamera::InfoConnect, this),
    boost::bind( &GazeboRosStereoCamera::InfoDisconnect, this));
#endif
}

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

// Increment count
void GazeboRosStereoCamera::PointCloudConnect()
{
  this->pointCloudConnectCount++;
}
// Decrement count
void GazeboRosStereoCamera::PointCloudDisconnect()
{
  this->pointCloudConnectCount--;

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

// Increment count
void GazeboRosStereoCamera::ImageConnect()
{
  this->imageConnectCount++;
}
// Decrement count
void GazeboRosStereoCamera::ImageDisconnect()
{
  this->imageConnectCount--;

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

// Initialize the controller
void GazeboRosStereoCamera::InitChild()
{

    // sensor generation off by default
  this->myParent->SetActive(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")
  {
    this->type = sensor_msgs::image_encodings::BAYER_RGGB8;
    this->skip = 1;
  }
  else if (this->myParent->GetImageFormat() == "BAYER_BGGR8")
  {
    this->type = sensor_msgs::image_encodings::BAYER_BGGR8;
    this->skip = 1;
  }
  else if (this->myParent->GetImageFormat() == "BAYER_GBRG8")
  {
    this->type = sensor_msgs::image_encodings::BAYER_GBRG8;
    this->skip = 1;
  }
  else if (this->myParent->GetImageFormat() == "BAYER_GRBG8")
  {
    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;
  if (this->focal_length == 0)
    this->focal_length = ((double)this->width) / (2.0 *tan(this->myParent->GetHFOV().GetAsRadian()/2.0));
  
#ifdef USE_CBQ
  // start custom queue for camera
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosStereoCamera::CameraQueueThread,this ) );
#endif


}

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

  // as long as ros is connected, parent is active
  //ROS_ERROR("debug image count %d",this->imageConnectCount);
  if (!this->myParent->IsActive())
  {
    if ((this->pointCloudConnectCount > 0) || (this->imageConnectCount > 0))
      // do this first so there's chance for sensor to run 1 frame after activate
      this->myParent->SetActive(true);
  }
  else
  {
    this->PutCameraData();
  }

  if (this->infoConnectCount > 0)
    this->PublishCameraInfo();
}

// Finalize the controller
void GazeboRosStereoCamera::FiniChild()
{
  this->myParent->SetActive(false);
  this->rosnode_->shutdown();
#ifdef USE_CBQ
  this->camera_queue_.clear();
  this->camera_queue_.disable();
  this->callback_queue_thread_.join();
#endif

}





// Put laser data to the interface
void GazeboRosStereoCamera::PutCameraData()
{

  const unsigned char *src;
  const float* depths;
  //boost::recursive_mutex::scoped_lock mr_lock(*Simulator::Instance()->GetMRMutex());

  // Get a pointer to image data
  {
    //DIAGNOSTICTIMER(timer("gazebo_ros_camera: GetImageData",6));
    src = this->myParent->GetImageData(0);
    depths = this->myParent->GetDepthData(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 );
      // publish to ros
      this->image_pub_.publish(this->imageMsg);
    }
    this->lock.unlock();
  }

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

    fillDepthImage(this->pointCloudMsg,
              this->height,
              this->width,
              this->skip,
              (void*)depths );
    /*
    for (int i = 0 ; i < this->height; i++)
    for (int j = 0 ; j < this->width; i++) {
      ROS_ERROR("test %d %d %f",i,j,depths[j + i*this->width]);
    }
    */

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

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

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

    this->lock.unlock();
  }
}


bool
GazeboRosStereoCamera::fillDepthImage(sensor_msgs::PointCloud& point_cloud,
                             uint32_t rows_arg,
                             uint32_t cols_arg,
                             uint32_t step_arg,
                             void* data_arg)
{
  point_cloud.points.resize(0);
  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();

      geometry_msgs::Point32 point;

      point.y      = -depth * tan(yAngle);
      point.z      = -depth * tan(pAngle);
      point.x      =  depth;

      this->pointCloudMsg.points.push_back(point);
    }
  }
  this->pointCloudMsg.channels[0].values.push_back(1.0);
  return true;
}

// Put laser data to the interface
void GazeboRosStereoCamera::PublishCameraInfo()
{
  // fill CameraInfo
  this->cameraInfoMsg.header.frame_id = this->frameName;
  Time lastRenderTime = (dynamic_cast<OgreCamera*>(this->myParent))->GetLastRenderTime();
  //Time lastRenderTime = Simulator::Instance()->GetSimTime();
  this->cameraInfoMsg.header.stamp.sec = lastRenderTime.sec;
  this->cameraInfoMsg.header.stamp.nsec = lastRenderTime.nsec;
  this->cameraInfoMsg.height = this->height;
  this->cameraInfoMsg.width  = this->width;
  // distortion
#if ROS_VERSION_MINIMUM(1, 3, 0)
  this->cameraInfoMsg.distortion_model = "plumb_bob";
  this->cameraInfoMsg.D.resize(5);
#endif
  this->cameraInfoMsg.D[0] = this->distortion_k1;
  this->cameraInfoMsg.D[1] = this->distortion_k2;
  this->cameraInfoMsg.D[2] = this->distortion_k3;
  this->cameraInfoMsg.D[3] = this->distortion_t1;
  this->cameraInfoMsg.D[4] = this->distortion_t2;
  // original camera matrix
  this->cameraInfoMsg.K[0] = this->focal_length;
  this->cameraInfoMsg.K[1] = 0.0;
  this->cameraInfoMsg.K[2] = this->Cx;
  this->cameraInfoMsg.K[3] = 0.0;
  this->cameraInfoMsg.K[4] = this->focal_length;
  this->cameraInfoMsg.K[5] = this->Cy;
  this->cameraInfoMsg.K[6] = 0.0;
  this->cameraInfoMsg.K[7] = 0.0;
  this->cameraInfoMsg.K[8] = 1.0;
  // rectification
  this->cameraInfoMsg.R[0] = 1.0;
  this->cameraInfoMsg.R[1] = 0.0;
  this->cameraInfoMsg.R[2] = 0.0;
  this->cameraInfoMsg.R[3] = 0.0;
  this->cameraInfoMsg.R[4] = 1.0;
  this->cameraInfoMsg.R[5] = 0.0;
  this->cameraInfoMsg.R[6] = 0.0;
  this->cameraInfoMsg.R[7] = 0.0;
  this->cameraInfoMsg.R[8] = 1.0;
  // camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
  this->cameraInfoMsg.P[0] = this->focal_length;
  this->cameraInfoMsg.P[1] = 0.0;
  this->cameraInfoMsg.P[2] = this->Cx;
  this->cameraInfoMsg.P[3] = -this->focal_length * this->hack_baseline;
  this->cameraInfoMsg.P[4] = 0.0;
  this->cameraInfoMsg.P[5] = this->focal_length;
  this->cameraInfoMsg.P[6] = this->Cy;
  this->cameraInfoMsg.P[7] = 0.0;
  this->cameraInfoMsg.P[8] = 0.0;
  this->cameraInfoMsg.P[9] = 0.0;
  this->cameraInfoMsg.P[10] = 1.0;
  this->cameraInfoMsg.P[11] = 0.0;
  this->camera_info_pub_.publish(this->cameraInfoMsg);


}


#ifdef USE_CBQ

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

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


}

---

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