firewire_camera_driver_nodelet.cpp

Go to the documentation of this file.

#include "firewire_camera_driver_nodelet.h"
#include <pluginlib/class_list_macros.h>

FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), camera_manager(ros::NodeHandle(this->public_node_handle_)),
                                                                          it(new image_transport::ImageTransport(this->public_node_handle_)),Image_msg_(new sensor_msgs::Image)
{
  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->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::CameraInfoPtr camera_info(new sensor_msgs::CameraInfo(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->diagnosed_camera_image!=NULL)
  {
    delete this->diagnosed_camera_image;
    this->diagnosed_camera_image=NULL;
  }
}

#include "firewire_camera_driver_nodelet.h"
#include <pluginlib/class_list_macros.h>

FirewireCameraNodelet::FirewireCameraNodelet()
{
  this->dev=NULL;
  // initialize the thread
  this->thread_server=CThreadServer::instance();
  this->spin_thread_id="firewire_camera_driver_nodelet_spin";
  this->thread_server->create_thread(this->spin_thread_id);
  this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
}

void FirewireCameraNodelet::onInit()
{
  ros::NodeHandle priv_nh(getPrivateNodeHandle());
  this->dev=new FirewireCameraDriverNode(priv_nh);
  // start the spin thread
  this->thread_server->start_thread(this->spin_thread_id);
}

void *FirewireCameraNodelet::spin_thread(void *param)
{
  FirewireCameraNodelet *nodelet=(FirewireCameraNodelet *)param;

  nodelet->dev->spin();

  pthread_exit(NULL);
}

FirewireCameraNodelet::~FirewireCameraNodelet()
{
  // kill the thread
  this->thread_server->kill_thread(this->spin_thread_id);
  this->thread_server->detach_thread(this->spin_thread_id);
  this->thread_server->delete_thread(this->spin_thread_id);
  this->spin_thread_id="";
  if(this->dev!=NULL)
    delete this->dev;
}

// parameters are: package, class name, class type, base class type
PLUGINLIB_DECLARE_CLASS(iri_firewire_camera, iri_firewire_nodelet, FirewireCameraNodelet, nodelet::Nodelet);