darwin_camera_driver_node.cpp

Go to the documentation of this file.

#include "darwin_camera_driver_node.h"
#include "Camera.h"

DarwinCameraDriverNode::DarwinCameraDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<DarwinCameraDriver>(nh), 
  camera_manager(ros::NodeHandle(this->public_node_handle_))
{
  //init class attributes if necessary
  this->loop_rate_ = 10;//in [Hz]
  this->it=new image_transport::ImageTransport(this->public_node_handle_);

  // [init publishers]
  this->camera_image_publisher_ = this->it->advertiseCamera("camera_image", 1);

  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

void DarwinCameraDriverNode::mainNodeThread(void)
{
  Image *new_image=NULL;
  int i=0;

  //lock access to driver if necessary
  this->driver_.lock();

  // [fill msg Header if necessary]
  //<publisher_name>.header.stamp = ros::Time::now();
  //<publisher_name>.header.frame_id = "<publisher_topic_name>";

  // [fill msg structures]
  
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // get a frame from the camera
  new_image=this->driver_.get_image();
  if(new_image!=NULL)
  {
    // configure Image message
    this->Image_msg_.encoding="rgb8";
    this->Image_msg_.width=new_image->m_Width;
    this->Image_msg_.height=new_image->m_Height;
    this->Image_msg_.step=new_image->m_WidthStep;
    this->Image_msg_.data.resize(new_image->m_ImageSize);
    for(i=0;i<new_image->m_ImageSize;i++)
      this->Image_msg_.data[i]=new_image->m_ImageData[i];

    // [publish messages]
    this->Image_msg_.header.stamp = ros::Time::now();
    this->camera_image_publisher_.publish(this->Image_msg_,this->camera_manager.getCameraInfo());
    delete new_image;
  }
  //unlock access to driver if previously blocked
  this->driver_.unlock();
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void DarwinCameraDriverNode::postNodeOpenHook(void)
{
}

void DarwinCameraDriverNode::addNodeDiagnostics(void)
{
}

void DarwinCameraDriverNode::addNodeOpenedTests(void)
{
}

void DarwinCameraDriverNode::addNodeStoppedTests(void)
{
}

void DarwinCameraDriverNode::addNodeRunningTests(void)
{
}

void DarwinCameraDriverNode::reconfigureNodeHook(int level)
{
}

DarwinCameraDriverNode::~DarwinCameraDriverNode(void)
{
  // [free dynamic memory]
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<DarwinCameraDriverNode>(argc, argv, "darwin_camera_driver_node");
}