bumblebee2_driver_node.cpp

Go to the documentation of this file.

#include "bumblebee2_driver_node.h"

Bumblebee2DriverNode::Bumblebee2DriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<Bumblebee2Driver>(nh),
                                                                  camera_manager(ros::NodeHandle(this->public_node_handle_))
{
  std::vector<std::string> cal_files;

  //init class attributes if necessary
  this->loop_rate_ = 1000;//in [Hz]
  this->desired_framerate=10;
  this->cameras.resize(NUM_CAM);
  this->cameras[LEFT].it=NULL;
  this->cameras[RIGHT].it=NULL;

  this->cameras[LEFT].it=new image_transport::ImageTransport(this->public_node_handle_);
  this->cameras[RIGHT].it=new image_transport::ImageTransport(this->public_node_handle_);

  this->event_server=CEventServer::instance();

  // [init publishers]
  this->cameras[LEFT].camera_image_publisher_ = this->cameras[LEFT].it->advertiseCamera("left_image", 1);
  this->cameras[RIGHT].camera_image_publisher_ = this->cameras[RIGHT].it->advertiseCamera("right_image", 1);

  cal_files.resize(2);
  public_node_handle_.param<std::string>("left_cal_file", cal_files[LEFT], "");
  public_node_handle_.param<std::string>("right_cal_file", cal_files[RIGHT], "");
  if(this->camera_manager.validateURL(cal_files[LEFT]))
  {
    this->camera_manager.setCameraName("bumblebee_left");
    if(this->camera_manager.loadCameraInfo(cal_files[LEFT]))
    {
      this->cameras[LEFT].CameraInfo_msg_=this->camera_manager.getCameraInfo();
      ROS_INFO("Found calibration file for the left camera: %s",cal_files[LEFT].c_str());
    }
    else
      ROS_INFO("Invalid calibration file for the left camera");
  }
  else
    ROS_INFO("Invalid calibration file for the left camera");

  if(this->camera_manager.validateURL(cal_files[RIGHT]))
  {
    this->camera_manager.setCameraName("bumblebee_right");
    if(this->camera_manager.loadCameraInfo(cal_files[RIGHT]))
    {
      this->cameras[RIGHT].CameraInfo_msg_=this->camera_manager.getCameraInfo();
      ROS_INFO("Found calibration file for the right camera: %s",cal_files[RIGHT].c_str());
    }
    else
      ROS_INFO("Invalid calibration file for the right camera");
  }
  else
    ROS_INFO("Invalid calibration file for the right camera");

  public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");

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

void Bumblebee2DriverNode::mainNodeThread(void)
{
  std::vector<std::string> frame_ids;
  std::list<std::string> events;
  char *left_image=NULL,*right_image=NULL;
  TCameraConfig config;
  int depth;

  try{
    //lock access to driver if necessary
    this->driver_.lock();
    if(this->driver_.isRunning())
    {
      this->new_frame_event=this->driver_.get_new_frame_event();
      if(this->new_frame_event!="")
      {
        events.push_back(this->new_frame_event);
        this->event_server->wait_all(events,1000);
        this->driver_.get_images(&left_image,&right_image);
        this->driver_.get_current_config(&config);
        this->driver_.get_frame_ids(frame_ids);
        depth=(int)ceil((float)config.depth/8.0);
        if(left_image!=NULL && this->cameras[LEFT].camera_image_publisher_.getNumSubscribers()>0)
        {
          //fill msg structures
          this->cameras[LEFT].Image_msg_.width=config.width;
          this->cameras[LEFT].Image_msg_.height=config.height;
          if(config.coding==RGB)
          {
            this->cameras[LEFT].Image_msg_.step=config.width*depth;
            this->cameras[LEFT].Image_msg_.data=std::vector<unsigned char>(left_image,left_image+config.width*config.height*depth);
            this->cameras[LEFT].Image_msg_.encoding="rgb8";
          }
          else
          {
            this->cameras[LEFT].Image_msg_.step=config.width*depth;
            this->cameras[LEFT].Image_msg_.data=std::vector<unsigned char>(left_image,left_image+config.width*config.height*depth);
            this->cameras[LEFT].Image_msg_.encoding="8UC1";
          }
          this->cameras[LEFT].Image_msg_.header.stamp = ros::Time::now();
          this->cameras[LEFT].Image_msg_.header.frame_id = tf_prefix_ + "/" + frame_ids[LEFT];
          this->cameras[LEFT].CameraInfo_msg_.header.stamp = this->cameras[LEFT].Image_msg_.header.stamp;
          this->cameras[LEFT].CameraInfo_msg_.header.frame_id = tf_prefix_ + "/" + frame_ids[LEFT];
          this->cameras[LEFT].camera_image_publisher_.publish(this->cameras[LEFT].Image_msg_,this->cameras[LEFT].CameraInfo_msg_);
        }
        delete[] left_image;
        if(right_image!=NULL && this->cameras[RIGHT].camera_image_publisher_.getNumSubscribers()>0)
        {
          //fill msg structures
          this->cameras[RIGHT].Image_msg_.width=config.width;
          this->cameras[RIGHT].Image_msg_.height=config.height;
          if(config.coding==RGB)
          {
            this->cameras[RIGHT].Image_msg_.step=config.width*depth;
            this->cameras[RIGHT].Image_msg_.data=std::vector<unsigned char>(right_image,right_image+config.width*config.height*depth);
            this->cameras[RIGHT].Image_msg_.encoding="rgb8";
          }
          else
          {
            this->cameras[RIGHT].Image_msg_.step=config.width*depth;
            this->cameras[RIGHT].Image_msg_.data=std::vector<unsigned char>(right_image,right_image+config.width*config.height*depth);
            this->cameras[RIGHT].Image_msg_.encoding="8UC1";
          }
          this->cameras[RIGHT].Image_msg_.header.stamp = ros::Time::now();
          this->cameras[RIGHT].Image_msg_.header.frame_id = tf_prefix_ + "/" + frame_ids[RIGHT];
          this->cameras[RIGHT].CameraInfo_msg_.header.stamp = this->cameras[RIGHT].Image_msg_.header.stamp;
          this->cameras[RIGHT].CameraInfo_msg_.header.frame_id = tf_prefix_ + "/" + frame_ids[RIGHT];
          this->cameras[RIGHT].camera_image_publisher_.publish(this->cameras[RIGHT].Image_msg_,this->cameras[RIGHT].CameraInfo_msg_);
        }
        delete[] right_image;
      }
    }
    this->driver_.unlock();
  }catch(CException &e){
    this->driver_.unlock();
    ROS_INFO("Impossible to capture frame");
  }
  //fill srv structure and make request to the server

  //publish messages
}

/*subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void Bumblebee2DriverNode::postNodeOpenHook(void)
{
}

void Bumblebee2DriverNode::addNodeDiagnostics(void)
{
}

void Bumblebee2DriverNode::addNodeOpenedTests(void)
{
}

void Bumblebee2DriverNode::addNodeStoppedTests(void)
{
}

void Bumblebee2DriverNode::addNodeRunningTests(void)
{
}

void Bumblebee2DriverNode::reconfigureNodeHook(int level)
{
}

Bumblebee2DriverNode::~Bumblebee2DriverNode(void)
{
  // [free dynamic memory]
  if(this->cameras[LEFT].it!=NULL)
  {
    delete this->cameras[LEFT].it;
    this->cameras[LEFT].it=NULL;
  }
  if(this->cameras[RIGHT].it!=NULL)
  {
    delete this->cameras[RIGHT].it;
    this->cameras[RIGHT].it=NULL;
  }
}

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