ladybug2_driver_node.cpp

Go to the documentation of this file.

#include "ladybug2_driver_node.h"

Ladybug2DriverNode::Ladybug2DriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<Ladybug2Driver>(nh),
  camera_manager(ros::NodeHandle(this->public_node_handle_))
{
  int i=0;
  std::vector<std::string> cal_files;

  //init class attributes if necessary
  this->loop_rate_ = 1000;//in [Hz]
  this->desired_framerate=10.0;
  this->cameras.resize(NUM_CAM);
  this->driver_.get_calibration_files(cal_files);
  cal_files.resize(NUM_CAM);
  public_node_handle_.param<std::string>("front_cal_file", cal_files[FRONT], "");
  public_node_handle_.param<std::string>("front_right_cal_file", cal_files[FRONT_RIGHT], "");
  public_node_handle_.param<std::string>("rear_right_cal_file", cal_files[REAR_RIGHT], "");
  public_node_handle_.param<std::string>("rear_left_cal_file", cal_files[REAR_LEFT], "");
  public_node_handle_.param<std::string>("front_left_cal_file", cal_files[FRONT_LEFT], "");
  public_node_handle_.param<std::string>("top_cal_file", cal_files[TOP], "");
  for(i=0;i<NUM_CAM;i++)
  {
    this->cameras[i].it=NULL;
    this->cameras[i].it=new image_transport::ImageTransport(this->public_node_handle_);
    // try to load the calibration file
    if(this->camera_manager.validateURL(cal_files[i]))
    {
      if(this->camera_manager.loadCameraInfo(cal_files[i]))
      {
        this->cameras[i].CameraInfo_msg_=this->camera_manager.getCameraInfo();
        ROS_INFO("Found calibration file for the camera: %s",cal_files[i].c_str());
      }
      else
        ROS_INFO("Invalid calibration file for the camera");
    }
    else
      ROS_INFO("Invalid calibration file for the camera");
  }

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

  // [init publishers]
  this->cameras[FRONT].camera_image_publisher_ = this->cameras[FRONT].it->advertiseCamera("front_image", 1);
  this->cameras[FRONT_RIGHT].camera_image_publisher_ = this->cameras[FRONT_RIGHT].it->advertiseCamera("front_right_image", 1);
  this->cameras[REAR_RIGHT].camera_image_publisher_ = this->cameras[REAR_RIGHT].it->advertiseCamera("rear_right_image", 1);
  this->cameras[REAR_LEFT].camera_image_publisher_ = this->cameras[REAR_LEFT].it->advertiseCamera("rear_left_image", 1);
  this->cameras[FRONT_LEFT].camera_image_publisher_ = this->cameras[FRONT_LEFT].it->advertiseCamera("front_left_image", 1);
  this->cameras[TOP].camera_image_publisher_ = this->cameras[TOP].it->advertiseCamera("top_image", 1);

  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 Ladybug2DriverNode::mainNodeThread(void)
{
  std::vector<sensor_msgs::CameraInfo> camera_info(NUM_CAM);
  std::vector<std::string> frame_ids;
  std::list<std::string> events;
  char *images[NUM_CAM]={NULL,NULL,NULL,NULL,NULL,NULL};
  TCameraConfig config;
  int i=0,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(&images[FRONT],&images[FRONT_RIGHT],&images[REAR_RIGHT],&images[REAR_LEFT],&images[FRONT_LEFT],&images[TOP]);
        this->driver_.get_current_config(&config);
        this->driver_.get_frame_ids(frame_ids);
        depth=(int)ceil((float)config.depth/8.0);
        for(i=0;i<NUM_CAM;i++)
        {
          if(images[i]!=NULL && this->cameras[i].camera_image_publisher_.getNumSubscribers()>0) 
          {
            //fill msg structures
            this->cameras[i].Image_msg_.width=config.width;
            this->cameras[i].Image_msg_.height=config.height;
            if(config.coding==RGB)
            { 
              this->cameras[i].Image_msg_.step=config.width*depth;
              this->cameras[i].Image_msg_.data=std::vector<unsigned char>(images[i],images[i]+config.width*config.height*depth);
              this->cameras[i].Image_msg_.encoding="rgb8";
            }
            else
            {
              this->cameras[i].Image_msg_.step=config.width*depth;
              this->cameras[i].Image_msg_.data=std::vector<unsigned char>(images[i],images[i]+config.width*config.height*depth);
              this->cameras[i].Image_msg_.encoding="8UC1";
            }
          }
          delete[] images[i];
          //publish messages
          this->cameras[i].Image_msg_.header.stamp = ros::Time::now();
          this->cameras[i].Image_msg_.header.frame_id = tf_prefix_ + "/" + frame_ids[i];
          this->cameras[i].CameraInfo_msg_.header.stamp = this->cameras[i].Image_msg_.header.stamp;
          this->cameras[i].CameraInfo_msg_.header.frame_id = tf_prefix_ + "/" + frame_ids[i];
          this->cameras[i].camera_image_publisher_.publish(this->cameras[i].Image_msg_,camera_info[i]);
        }
      }
    }
    this->driver_.unlock();
  }catch(CException &e){
    this->driver_.unlock();
    ROS_INFO("Impossible to capture frame");
  }
  //fill srv structure and make request to the server
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void Ladybug2DriverNode::postNodeOpenHook(void)
{
}

void Ladybug2DriverNode::addNodeDiagnostics(void)
{
}

void Ladybug2DriverNode::addNodeOpenedTests(void)
{
}

void Ladybug2DriverNode::addNodeStoppedTests(void)
{
}

void Ladybug2DriverNode::addNodeRunningTests(void)
{
}

void Ladybug2DriverNode::reconfigureNodeHook(int level)
{
}

Ladybug2DriverNode::~Ladybug2DriverNode(void)
{
  int i=0;

  // [free dynamic memory]
  for(i=0;i<NUM_CAM;i++)
  {
    if(this->cameras[i].it!=NULL)
    {
      delete this->cameras[i].it;
      this->cameras[i].it=NULL;
    }
  }
}

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