git_ws/grapher/src/grapher_node.cpp

#include "scanGrapher.h"

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <pcl/common/transforms.h>	
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <boost/thread.hpp>


////// thread_queue
#include "./TaskQueue/TaskPool.h"
#include "./TaskQueue/TQFactory.h"
#include "locateTask.h"
#include <cstdio>
#include <thread>
using namespace tq;



#include <chrono>
#undef max()
#undef min()
#include "kalmanFilter.h"
#include "cfg/grapher_cfg.pb.h"
#include "cfg/protobuf_io.h"

grapher::grapherCfg   grapher_config;

ros::Publisher pubCloudL;
ros::Publisher pubCloudG;
ros::Publisher grid_pub;


pcl::PointXYZ minp=pcl::PointXYZ(-10.0,-10.0,0);
pcl::PointXYZ maxp=pcl::PointXYZ(10.0,10.0,0);
double resolution=0.01;    
ScanGrapher* pGrapher=new ScanGrapher(minp,maxp,resolution);
KalmanFilter klmFilter;

pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);

/* void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,LDP& ldp)
{
  unsigned int n = scan_msg->ranges.size();
  ldp = ld_alloc_new(n);

  for (unsigned int i = 0; i < n; i++)
  {
    // calculate position in laser frame

    double r = scan_msg->ranges[i];

    if (r > scan_msg->range_min && r < scan_msg->range_max)
    {
      // fill in laser scan data

      ldp->valid[i] = 1;
      ldp->readings[i] = r;
    }
    else
    {
      ldp->valid[i] = 0;
      ldp->readings[i] = -1;  // for invalid range
    }

    ldp->theta[i]    = scan_msg->angle_min + i * scan_msg->angle_increment;

    ldp->cluster[i]  = -1;
  }

  ldp->min_theta = ldp->theta[0];
  ldp->max_theta = ldp->theta[n-1];

  ldp->odometry[0] = 0.0;
  ldp->odometry[1] = 0.0;
  ldp->odometry[2] = 0.0;

  ldp->true_pose[0] = 0.0;
  ldp->true_pose[1] = 0.0;
  ldp->true_pose[2] = 0.0;
}*/

pcl::PointCloud<pcl::PointXYZ>::Ptr transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input,tf::Pose pose)
{
    Eigen::Matrix4f transform=TFToEigen(pose);
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::transformPointCloud (*input, *transformed_cloud, transform);
    return transformed_cloud;

}

void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    static int systemInitCount=0;
    static bool systemInited=false;
    int systemDelay=20;
    if (!systemInited) 
    {//丢弃前20个点云数据
        systemInitCount++;
        if (systemInitCount >= systemDelay) {
        systemInited = true;
        }
        return;
    }

     //当前点云时间
    double timeScanCur = msg->header.stamp.toSec();
    int N = int( (msg->angle_max-msg->angle_min)/msg->angle_increment )+1;
    pcl::PointCloud<pcl::PointXYZ>  laserCloud1;
    for(int i=0;i<N;++i)
    {
        float distance = msg->ranges[i];
        
        float angle = msg->angle_min+ i*msg->angle_increment;
        if(distance >20.0)
          continue;
        //第一帧 用作 target  使用所有点，非第一帧只保留前方一半点
        pcl::PointXYZ point ;
        point.x= -sin(angle-M_PI_2)*distance;
        point.y= cos(angle-M_PI_2)*distance;
        point.z=0;
        if(fabs(point.x)>1.0 || fabs(point.y)>1.0)
        {   
          laserCloud1.points.push_back(point);
        }
    }
    if(laserCloud1.size()<10)
        return ;

    if(systemInitCount<systemDelay+20)
    {
    pGrapher->pushScan(laserCloud1.makeShared(),pcl::PointXYZ(0,0,0));
    systemInitCount++;
    return ;
    }
    static bool first_frame=true;
    if(first_frame==true)
    {
      pGrapher->getCloud(*target_cloud);
      if(target_cloud->size()==0)
      {
        return ;
      }
      
      first_frame=false;
    }
 
 
    static tf::Pose init_pose=tf::Pose::getIdentity();
    //线程队列
    IQueue* q = TQFactory::CreateDefaultQueue();
    q->Start(grapher_config.grapher_functions_size());
    std::vector<LocateTask*>  pTasks;
    for(int i=0;i<grapher_config.grapher_functions_size();++i)
    {
      LocateTask* pTask=0;
      if(grapher_config.grapher_functions(i).type()=="ndt")
        pTask=new NdtTask(laserCloud1.makeShared(),target_cloud,init_pose); // ndt 算法 
      else if(grapher_config.grapher_functions(i).type()=="icp")
        pTask=new IcpTask(laserCloud1.makeShared(),target_cloud,init_pose); // icp 算法 
      else  continue;
      pTasks.push_back(pTask);
      q->AddTask(pTask);
    }
    q->WaitForFinish();
   
    //获取结果，发布tf
    static tf::TransformBroadcaster tfBroadcaster;
    std::vector<tf::Pose> poses;
    tf::Pose lastPose=tf::Pose::getIdentity();
    for(int i=0;i<grapher_config.grapher_functions_size();++i)
    {
      LocateTask* pTask=pTasks[i];
      tf::Pose pose;
      if(pTask->GetResult(pose))
      {
        lastPose=pose;
        poses.push_back(pose);
        std::string header_id=grapher_config.grapher_functions(i).tfheaderid();
        std::string frame_id=grapher_config.grapher_functions(i).tfid();
        
        tf::StampedTransform laserOdometryTrans;
        laserOdometryTrans.frame_id_ = header_id;
        laserOdometryTrans.child_frame_id_ = frame_id;
        laserOdometryTrans.stamp_ = ros::Time::now(); 
        laserOdometryTrans.setRotation(pose.getRotation());
        laserOdometryTrans.setOrigin(pose.getOrigin());
        tfBroadcaster.sendTransform(laserOdometryTrans);
      }
      //析构
      delete pTask;
    }
    TQFactory::ReleaseQueue(q);
    //滤波
    if(poses.size()==2)
      lastPose=klmFilter.filter(poses[0],poses[1]);
//    lastPose = poses[0];
      
    tf::StampedTransform laserOdometryTrans;
    laserOdometryTrans.frame_id_ = "/map";
    laserOdometryTrans.child_frame_id_ = "/laser";
    laserOdometryTrans.stamp_ =ros::Time::now(); 
    laserOdometryTrans.setRotation(lastPose.getRotation());
    laserOdometryTrans.setOrigin(lastPose.getOrigin());
    tfBroadcaster.sendTransform(laserOdometryTrans);

    ///变换点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_T=transformCloud(laserCloud1.makeShared(),lastPose);
    pGrapher->pushScan(cloud_T,pcl::PointXYZ(lastPose.getOrigin().x(),lastPose.getOrigin().y(),0));
    init_pose=lastPose;

    sensor_msgs::PointCloud2 cloudLocalMsg;
    pcl::toROSMsg(*cloud_T, cloudLocalMsg);
    cloudLocalMsg.header.stamp = msg->header.stamp;
    cloudLocalMsg.header.frame_id = "/map";
    pubCloudL.publish(cloudLocalMsg);


}

void transforMatToGrid(cv::Mat& image,nav_msgs::OccupancyGrid& map)
{

  map.header.frame_id="map";
  map.header.stamp = ros::Time::now(); 
  map.info.resolution = pGrapher->resolution_;         // float32
  map.info.width      = image.cols;           // uint32
  map.info.height     = image.rows;           // uint32
  map.info.origin.position.x=-image.cols*pGrapher->resolution_/2.0;
  map.info.origin.position.y=-image.rows*pGrapher->resolution_/2.0;
  static signed char* pdata=0; 
  if(pdata!=0) free(pdata);
  pdata=(signed char*)malloc(image.cols*image.rows*sizeof(signed char));
  for(int i=0;i<image.rows;++i)
  {
    for(int j=0;j<image.cols;++j)
    {
      if(image.at<uchar>(i,j)==0)
          pdata[i*image.cols+j]=-1;
      else if(image.at<uchar>(i,j)<10)
          pdata[i*image.cols+j]=0;
      else
        pdata[i*image.cols+j]=int(double(image.at<uchar>(i,j))*(100./255.0));
    }
  }
  map.data = std::vector<signed char>(pdata,pdata+image.cols*image.rows);

}

void publish()
{
    ros::Rate loop_rate(10);
    
    int last_cloud_size=-1;
    while(ros::ok())
    {
      
        cv::Mat grid=pGrapher->getGrid();
        nav_msgs::OccupancyGrid map;
        transforMatToGrid(grid,map);
        grid_pub.publish(map);

        ///发布地图点云
        /*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=pGrapher->getCloud();
        if(cloud->size()>0)
        {
          sensor_msgs::PointCloud2 cloudglobalMsg;
          pcl::toROSMsg(*cloud, cloudglobalMsg);
          cloudglobalMsg.header.stamp = ros::Time::now(); 
          cloudglobalMsg.header.frame_id = "/map";
          pubCloudG.publish(cloudglobalMsg);
        }*/

        ros::spinOnce();
        loop_rate.sleep();
    }
}

//#include <stdio.h>
int main(int argc, char** argv)
{
//    if(false==ReadProtoConfig("/home/zx/zzw/catkin_ws/src/grapher/grapher.cfg",grapher_config))
    if(false==ReadProtoConfig("/home/youchen/Documents/git_ws/src/grapher/grapher.cfg",grapher_config))
    {
      printf(" config file load failed ... \n");
      return -1;
    }

    ros::init(argc, argv, "grapher_node");
    ros::NodeHandle nh;

    
    pubCloudL = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/cloud_local", 2);
    pubCloudG = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/cloud_global", 2);
    grid_pub=nh.advertise<nav_msgs::OccupancyGrid>("/grid",1);
    //pcl::io::loadPCDFile("/home/zx/zzw/ws/src/grapher/map.pcd", *target_cloud);
    //std::cout<<" target cloud size :"<<target_cloud->size()<<std::endl;

    boost::thread* thread_=new boost::thread(publish);

    std::string scanID=grapher_config.scanid();
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::LaserScan>(scanID, 2,scanHandler);

    ros::spin();

//    pGrapher->saveImage("/home/zx/zzw/map.bmp");
//    pGrapher->saveCloudTxt("/home/zx/zzw/map.txt");
//    pGrapher->saveCloudPcd("/home/zx/zzw/map.pcd");
    pGrapher->saveImage("/home/youchen/Documents/git_ws/src/grapher/map.bmp");
    pGrapher->saveCloudTxt("/home/youchen/Documents/git_ws/src/grapher/map.txt");
    pGrapher->saveCloudPcd("/home/youchen/Documents/git_ws/src/grapher/map.pcd");
    delete pGrapher;
    

    return 0;
}