my_database_publ_alg_node.cpp

Go to the documentation of this file.

#include "my_database_publ_alg_node.h"

MyDatabasePublAlgNode::MyDatabasePublAlgNode(void) :
        algorithm_base::IriBaseAlgorithm<MyDatabasePublAlgorithm>() ,
        it_(this->public_node_handle_),
        iit_database(0),
    tf_listener_(ros::Duration(30.f))
{
  //init class attributes if necessary
  this->loop_rate_ = 10;//in [Hz]

  // [init publishers]
  this->goal_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseArray>("goal", 100);
        image_pub_ = it_.advertiseCamera("image_raw", 1);
  this->scan_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan", 100);
  this->scan_proc_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan_proc", 100);
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  init_sensor_info();
}

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

void MyDatabasePublAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->PoseArray_msg_.data = my_var;
  //this->CameraInfo_msg.data = my_var;
        if (iit_database < 2157) //database length
                update_msgs();  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  this->goal_publisher_.publish(this->PoseArray_msg_);
        sensor_msgs::Image image_;
        this->img_msg_.toImageMsg( image_ );
        this->image_pub_.publish( image_,  this->CameraInfo_msg_ );
        this->scan_publisher_.publish(this->LaserScan_msg_);
        this->scan_proc_publisher_.publish(this->LaserScan_proc_msg_);
}

void MyDatabasePublAlgNode::init_sensor_info()
{
        //Image
        img_msg_.header.frame_id = "/camera";

        //      Camera Info -----------------------------------------------------
        CameraInfo_msg_.height = 384;
        CameraInfo_msg_.width = 512;
        CameraInfo_msg_.distortion_model = "plumb_bob";
//      kc << -0.202358, 0.053583, 0.000246051, -0.00136075, 0;
        CameraInfo_msg_.D.clear();
        CameraInfo_msg_.D.push_back(-0.202358);
        CameraInfo_msg_.D.push_back( 0.053583);
        CameraInfo_msg_.D.push_back( 0.000246051);
        CameraInfo_msg_.D.push_back( -0.00136075);
        CameraInfo_msg_.D.push_back( 0);

//       K << 1324.17, 0, 518.221,
//              0, 1323.21, 393.857,
//              0, 0, 1;
        CameraInfo_msg_.K[0] =  1324.17;
        CameraInfo_msg_.K[2] = 518.221;
        CameraInfo_msg_.K[4] =  1323.21;
        CameraInfo_msg_.K[5] = 393.857;
        CameraInfo_msg_.K[8] = 1;
// float64[9] CameraInfo_msg_.R
        CameraInfo_msg_.R[0] =  1.0;
        CameraInfo_msg_.R[4] =  1;
        CameraInfo_msg_.R[8] =  1.0;

// float64[12] CameraInfo_msg_.P := K *[R | t]
        CameraInfo_msg_.P[0] =  1324.17*0.5;
        CameraInfo_msg_.P[2] = 518.221*0.5;
        CameraInfo_msg_.P[5] =  1323.21*0.5;
        CameraInfo_msg_.P[6] = 393.857*0.5;
        CameraInfo_msg_.P[10] = 1;

        CameraInfo_msg_.header.frame_id = "/camera";
    cam_model_.fromCameraInfo(CameraInfo_msg_);


    //Laser params
        LaserScan_msg_.header.frame_id = "/laser";
        LaserScan_msg_.angle_min = 0.69813; //40 deg
        LaserScan_msg_.angle_max = 2.4478; //100+40 deg
        LaserScan_msg_.angle_increment = 0.00436332309619; //0.25º
        LaserScan_msg_.time_increment = 0.0000173611115315; //?
        LaserScan_msg_.scan_time = 0.0250000003725; //?
        LaserScan_msg_.range_min = 0.0230000000447;
        LaserScan_msg_.range_max = 60.0;

    //Laser cleaned params
        LaserScan_proc_msg_.header.frame_id = "/laser";
        LaserScan_proc_msg_.angle_min = 1.2086; //69.25 deg
        LaserScan_proc_msg_.angle_max = 1.9635; //112.5
        LaserScan_proc_msg_.angle_increment = 0.00436332309619; //0.25º
        LaserScan_proc_msg_.time_increment = 0.0000173611115315; //?
        LaserScan_proc_msg_.scan_time = 0.0250000003725; //?
        LaserScan_proc_msg_.range_min = 0.0230000000447;
        LaserScan_proc_msg_.range_max = 60.0;


        //annotations .txt
        std::string homepath = getenv("HOME");
        std::string fileString = homepath + "/Desktop/laser_vision_pr/annotations/annotations_test.txt";
        //std::ifstream infile;
        //infile.open (fileString.c_str() , ifstream::in)
        int num_det, x, y, w, h, cont;
        char c[256];
        std::vector<cv::Rect> instant_detections;
        //infile.getline (c,256);
        //while (infile.good()){
                //infile.getline (c,256);
        //}
        //infile.close();
        
        FILE * file_out;
        file_out = fopen (  fileString.c_str() , "r");
        fscanf(file_out, "%s " , c);
        //while(  !feof(file_out) ){
        for (unsigned int j = 0; j<2157; j++){
                fscanf (file_out, "%d", &num_det);
                instant_detections.clear();
                //ROS_INFO("LASER SCAN: %d ", j);
                for( int i =0 ; i < num_det; i++){
                        fscanf(file_out, " %d %d %d %d", &x , &y, &w, &h);
                        instant_detections.push_back( cv::Rect(x/2,y/2,w/2,h/2) );
                }
                box_detections.push_back( instant_detections );
                
                fscanf(file_out, "\n%s " , c);
                cont++;
        }
        fclose(file_out);

}

void MyDatabasePublAlgNode::update_msgs()
{
        cv::Mat outIm;

    ros::Time now = ros::Time::now();
        iit_database ++;
        //      Header
        CameraInfo_msg_.header.seq   = iit_database;
        CameraInfo_msg_.header.stamp = now;
        img_msg_.header.seq          = iit_database;
        img_msg_.header.stamp        = now;
        LaserScan_msg_.header.seq    = iit_database;
        LaserScan_msg_.header.stamp  = now;
        LaserScan_proc_msg_.header.seq    = iit_database;
        LaserScan_proc_msg_.header.stamp  = now;

        //Image --------------------------------------------------------
        std::string homepath = getenv("HOME");
        std::string fileString = homepath + "/Desktop/laser_vision_pr/test/camera/list.lst";
        FILE * file_out;
        file_out = fopen (  fileString.c_str() , "r");
        fseek(file_out , 14*(iit_database -1) , SEEK_SET );
        char file_name[20];
        fscanf(file_out, "%s\n" , file_name); 
        fclose(file_out);
        std::string string_file_name = file_name;

        fileString = homepath + "/Desktop/laser_vision_pr/test/camera/" + string_file_name; //list.lst -> listed all file names...
        outIm = cv::imread( fileString.c_str(), 1 );
    img_msg_.encoding = "bgr8";
    img_msg_.image    = outIm;
        
        //Laser Scan --------------------------------------------------
        fileString = homepath + "/Desktop/laser_vision_pr/test/laser/list.lst";
        file_out = fopen (  fileString.c_str() , "r");
        fseek(file_out , 13*(iit_database -1) , SEEK_SET );
        fscanf(file_out, "%s\n" , file_name); 
        fclose(file_out);
        string_file_name = file_name;
        fileString = homepath + "/Desktop/laser_vision_pr/test/laser/" + string_file_name;
        file_out = fopen (  fileString.c_str() , "r");  
        

        float f;
        LaserScan_msg_.ranges.clear();  
        while (  !feof(file_out) ) {
                fscanf (file_out, "%f ", &f);
                LaserScan_msg_.ranges.push_back( f/100 );
        }
        //for(unsigned int i = 2 ; i< 220 ; i++)
        //LaserScan_msg_.ranges[200]=5.0;
        fclose(file_out );

        //processed laser scans appearing in the image
        LaserScan_proc_msg_.ranges.clear();
        for(unsigned int i = 116; i<290 ;i++)
                LaserScan_proc_msg_.ranges.push_back( LaserScan_msg_.ranges[i] );


        //cada iteracio
        img_dect det;
        std::vector<img_dect> img_detections;
        for ( unsigned int i = 0; i < box_detections[iit_database-1].size() ;  i++){
                det.id = i;
                det.Bbox = box_detections[iit_database-1][i];
                det.score = 1.0f;
                img_detections.push_back(det);
        }

        //ROS_INFO("box_detections: %d ",img_detections.size());


        std::vector<point2D3D> laserScans;
        tf::StampedTransform laser_to_camera_transf;

        try
        {
                ros::Time acquisition_time = LaserScan_msg_.header.stamp;
                ros::Duration timeout(1.0f / 30.f);

                // wait for transform from laser to camera 
                tf_listener_.waitForTransform(cam_model_.tfFrame(), LaserScan_msg_.header.frame_id,
                                                    acquisition_time, timeout);

                tf_listener_.lookupTransform(cam_model_.tfFrame(), LaserScan_msg_.header.frame_id,
                                                   acquisition_time, laser_to_camera_transf);


                //ROS_INFO("entering for : size = %d" , LaserScan_msg_.ranges.size());
                for(unsigned int ii=0; ii < LaserScan_msg_.ranges.size(); ii++)
                {
                        //read laser scan
                        tf::Point laser_point(LaserScan_msg_.ranges[ii]*cos(LaserScan_msg_.angle_min+LaserScan_msg_.angle_increment*ii),
                                                          LaserScan_msg_.ranges[ii]*sin(LaserScan_msg_.angle_min+LaserScan_msg_.angle_increment*ii),
                                                          0.f);
                        //ROS_INFO("point x = %f , y= %f, z= %f", laser_point.x(), laser_point.y(), laser_point.z() );
                        //transform laser 3d point to camera coordinates
                        tf::Point cam_point(laser_to_camera_transf * laser_point);
                        //ROS_INFO("camera point x = %f , y= %f, z= %f", cam_point.x(), cam_point.y(), cam_point.z() );
                        cv::Point3d cv_cam_point(cam_point.x(), cam_point.y(), cam_point.z());
                        //ROS_INFO("cv camera point x = %f , y= %f, z= %f", cv_cam_point.x, cv_cam_point.y, cv_cam_point.z );
                        // transform laser 3d camera point to image plane
                        cv::Point2d image_point = cam_model_.project3dToPixel(cv_cam_point);
                        //ROS_INFO("cv camera 2d point x = %f , y= %f", image_point.x, image_point.y );
                        //update vector
                        point2D3D p;
                        p.p3D.x = LaserScan_msg_.ranges[ii]*cos(LaserScan_msg_.angle_min+LaserScan_msg_.angle_increment*ii);
                        p.p3D.y = LaserScan_msg_.ranges[ii]*sin(LaserScan_msg_.angle_min+LaserScan_msg_.angle_increment*ii);
                        p.p3D.z = 0.0f;
                        p.p2D.x = image_point.x;
                        p.p2D.y = image_point.y;
                        laserScans.push_back( p );
                        //ROS_INFO("laser points x = %d , y= %d, z= %d", p.p3D.x, p.p3D.y, p.p3D.z );
                        //ROS_INFO("tf adq %d" , ii);
                }
                std::vector<las_dect> cam_laser_dets;
                cam_detector_.camtoLaser(img_detections, laserScans, cam_laser_dets);
                //ROS_INFO("camera laser dets size = %d",  cam_laser_dets.size() );
                //pose array msg
                PoseArray_msg_.header.seq   = iit_database;
                PoseArray_msg_.header.stamp = now;
                PoseArray_msg_.header.frame_id = "/laser";
                geometry_msgs::Pose pp;
                PoseArray_msg_.poses.clear();
                for( unsigned int i = 0; i < cam_laser_dets.size() ; i++){
                        pp.position.x = cam_laser_dets[i].point.p3D.x;
                        pp.position.y = cam_laser_dets[i].point.p3D.y;
                        pp.position.z = cam_laser_dets[i].point.p3D.z;
                        pp.orientation.z = 1.0f;
                        PoseArray_msg_.poses.push_back(pp);
                }
                //void drawpeopledetection(const cv::Mat& img, cv::Mat & outIm, const std::vector<cv::Rect>& found_filtered);
                //cam_detector_.drawpeopledetection(outIm, outIm, box_detections[iit_database-1] );
        //cam_detector_.drawalllaserdetections(outIm, outIm, laserScans);
                //img_msg_.image    = outIm;
        }
        catch (tf::TransformException& ex)
        {
                ROS_WARN("TF exception:\n%s", ex.what());
        }

}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void MyDatabasePublAlgNode::node_config_update(Config &config, uint32_t level)
{
  ROS_DEBUG("MyDatabasePublAlgNode::node_config_update");
  this->alg_.lock();
        iit_database = config.sequence_iterator;
        loop_rate_   = config.frame_rate;
  this->alg_.unlock();
}

void MyDatabasePublAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<MyDatabasePublAlgNode>(argc, argv, "my_database_publ_alg_node");
}