firewire_camera_driver_node.cpp

Go to the documentation of this file.

#include "firewire_camera_driver_node.h"

FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), camera_manager(ros::NodeHandle(this->public_node_handle_))
{
  std::string cal_file;

  //init class attributes if necessary
  this->loop_rate_ = 1000;//in [Hz]
  this->desired_framerate=30.0;
  this->diagnosed_camera_image=NULL;
  this->it=NULL;

  this->it=new image_transport::ImageTransport(this->public_node_handle_);
  this->diagnosed_camera_image = new diagnostic_updater::HeaderlessTopicDiagnostic("camera_image",this->diagnostic_,
                                     diagnostic_updater::FrequencyStatusParam(&this->desired_framerate,&this->desired_framerate,0.01,5));

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

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

  // try to load the calibration file
  public_node_handle_.param<std::string>("left_cal_file", cal_file, "");
  if(this->camera_manager.validateURL(cal_file))
  {
    if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
      ROS_INFO("Invalid calibration file");
  } 
  else
    ROS_INFO("Invalid calibration file");

  public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

void FirewireCameraDriverNode::mainNodeThread(void)
{
  sensor_msgs::CameraInfo camera_info=this->camera_manager.getCameraInfo();
  std::list<std::string> events;
  char *image_data=NULL;
  TCameraConfig config;

  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_image(&image_data);
        if(image_data!=NULL && this->camera_image_publisher_.getNumSubscribers()>0)
        {
          this->driver_.get_current_config(&config);
          // update the desired framerate for the diagnostics
          this->desired_framerate=config.framerate;
          //fill msg structures
          this->Image_msg_.width=config.width;
          this->Image_msg_.height=config.height;
          this->Image_msg_.step=config.width*config.depth/8;
          this->Image_msg_.data=std::vector<unsigned char>(image_data,image_data+config.width*config.height*config.depth/8);
          if(config.coding==MONO || config.coding==RAW)
          {
            if(config.depth==DEPTH8)
              this->Image_msg_.encoding="8UC1";
            else if(config.depth==DEPTH16)
              this->Image_msg_.encoding="16UC1";
            else
              this->Image_msg_.encoding="16UC1";
          }
          else if(config.coding==YUV)
          {
            this->Image_msg_.encoding="yuv422";
          }
          else
          {
            if(config.depth==DEPTH24)
              this->Image_msg_.encoding="rgb8";
            else if(config.depth==DEPTH48)
              this->Image_msg_.encoding="16UC3";
            else
              this->Image_msg_.encoding="16UC3";
          }
        }
        delete[] image_data;
        //publish messages
        this->Image_msg_.header.stamp = ros::Time::now();
        this->Image_msg_.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
        camera_info.header.stamp = this->Image_msg_.header.stamp;
        camera_info.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
        //this->Image_msg_.header.frame_id = "<publisher_topic_name>";
        this->camera_image_publisher_.publish(this->Image_msg_,camera_info);
        this->diagnosed_camera_image->tick();
      }
    }
    this->driver_.unlock();
  }catch(CException &e){
    this->driver_.unlock();
    ROS_INFO("Impossible to capture frame");
  }
  //fill srv structure and make request to the server
}
  // [fill msg Header if necessary]

void FirewireCameraDriverNode::check_configuration(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  TCameraConfig desired_config;
  TCameraConfig current_config;

  this->driver_.get_current_config(&current_config);
  this->driver_.get_desired_config(&desired_config);

  if(current_config.left_offset!=desired_config.left_offset)
  {
    stat.summary(1,"The current configuration differs from the desired configuration");
    stat.addf("Image left offset:","current value is %d,desired value is %d", current_config.left_offset,desired_config.left_offset);
  }
  else if(current_config.top_offset!=desired_config.top_offset)
  {
    stat.summary(1,"The current configuration differs from the desired configuration");
    stat.addf("Image top offset:","current value is %d,desired value is %d", current_config.top_offset,desired_config.top_offset);
  }
  else if(current_config.width!=desired_config.width)
  {
    stat.summary(1,"The current configuration differs from the desired configuration");
    stat.addf("Image width:","current value is %d,desired value is %d", current_config.width,desired_config.width);
  }
  else if(current_config.height!=desired_config.height)
  {
    stat.summary(1,"The current configuration differs from the desired configuration");
    stat.addf("Image height:","current value is %d,desired value is %d", current_config.height,desired_config.height);
  }
  else
    stat.summary(0,"The current configuration is okay");
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void FirewireCameraDriverNode::postNodeOpenHook(void)
{
}

void FirewireCameraDriverNode::addNodeDiagnostics(void)
{
  diagnostic_.add("Check Configuration", this, &FirewireCameraDriverNode::check_configuration);
}

void FirewireCameraDriverNode::addNodeOpenedTests(void)
{
}

void FirewireCameraDriverNode::addNodeStoppedTests(void)
{
}

void FirewireCameraDriverNode::addNodeRunningTests(void)
{
}

void FirewireCameraDriverNode::reconfigureNodeHook(int level)
{
  // try to load the calibration file
  if(this->camera_manager.validateURL(this->driver_.get_calibration_file()))
  {
    this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file());
  } 
  else
    ROS_INFO("Invalid calibration file");
}

FirewireCameraDriverNode::~FirewireCameraDriverNode()
{
  // [free dynamic memory]
  if(this->it!=NULL)
  {
    delete this->it;
    this->it=NULL;
  }
  if(this->diagnosed_camera_image!=NULL)
  {
    delete this->diagnosed_camera_image;
    this->diagnosed_camera_image=NULL;
  }
}

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