modularization/src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

#include <QCoreApplication>
#include "perception_lidar_vv7.h"
#include <getopt.h>
#include <thread>
#include <vector>
#include <list>
#include <iostream>
#include <QDateTime>
#include <QMutex>

#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "modulecomm.h"
#include "ivversion.h"
#include "ivbacktrace.h"

#include "perceptionoutput.h"

#include <iostream>
#include <fstream>
#include <yaml-cpp/yaml.h>
#include "ivfault.h"
#include "ivlog.h"


static void * glidar_pc;
static void * glidar_obs;
static void * g_lidar_pc_floor;
static void * g_lidar_pc_nofloor;

static void * glidar_pc_p;
static void * glidar_per;

static void * gpafusion;

iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

static char gstr_sensorheight[256];
static char gstr_vehicleheight[256];
static char gstr_inputmemname[256];
static char gstr_outputmemname[256];
static char gstr_skipxmin[256];
static char gstr_skipxmax[256];
static char gstr_skipymin[256];
static char gstr_skipymax[256];


static int g_robosys = 10;

//#define OUT_GROUND

namespace iv {
    const int grx = 200, gry = 550, centerx = 100, centery = 50;
    const double gridwide = 0.2;
    struct ObstacleBasic
    {
        bool valid;
        float nomal_x;
        float nomal_y;
        float nomal_z;

        float speed_relative;
        float speed_x;
        float speed_y;
        float speed_z;

        float high;
        float low;

        int esr_ID;
    };

//    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsLiDAR;
//    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsRadar;
//    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsCamera;
//    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>>ObsRadarPointer;
    struct Obs_grid
    {
        double high;
        double low;
        double obshight;
        double groundheight;
        int pointcount;
        int ob;//??????0?? ?1???2???
    };
    typedef Obs_grid LidarGrid;
    typedef Obs_grid* LidarGridPtr;
//    int xxxx;

}



static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void * pasm)
{
    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];

    int * pHeadSize = (int *)strOut;
    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
//    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));

    unsigned int * pu32 = (unsigned int *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
    *pu32 = point_cloud->header.seq;
    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));

//    std::cout<<"width  = "<<point_cloud->width<<std::endl;


    iv::modulecomm::ModuleSendMsg(pasm,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->size() * sizeof(pcl::PointXYZI));
    delete strOut;
}




static float SENSOR_HEIGHT = 1.9;
static float VEHICLE_HEIGHT = 2.2;
//static float local_max_slope_ = 10;
//static float general_max_slope_ = 3;
//static float concentric_divider_distance_ = 1.5;
//static float min_height_threshold_ = 0.05;
//static float reclass_distance_threshold_ = 10.0;


static float GRID_SIZE = 2.0;
static float SMALLGRID_SIZE = 0.2;
static float Height_Thesh_Ratio = 0.1;
static float General_Thresh_Ratio = 0.1;
static float VEC_LEN = 200.0;
static float HOR_LEN = 200.0;
static float VEC_MIN = -100.0;
static float HOR_MIN = -100.0;
static int g_seq = 0;

static float skip_xmin = -0.9;
static float skip_ymin = -2.3;
static float skip_xmax = 0.9;
static float skip_ymax = 2.3;

static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs)
{

//    std::cout<<"enter gird obs"<<std::endl;
    int nVecSize = VEC_LEN/GRID_SIZE;
    int nHorSize = HOR_LEN/GRID_SIZE;
    bool * floor_find = new bool[(nVecSize+2)*(nHorSize+2)];
    float * floor_h_raw = new float[(nVecSize+2)*(nHorSize+2)];
    float * floor_h = new float[nVecSize*nHorSize];


    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_nofloor(
                new pcl::PointCloud<pcl::PointXYZI>());

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_floor(
                new pcl::PointCloud<pcl::PointXYZI>());

    QDateTime dt = QDateTime::currentDateTime();

    point_cloud_floor->header.frame_id = "velodyne";
    point_cloud_floor->height = 1;
    point_cloud_floor->header.stamp = dt.currentMSecsSinceEpoch();
    point_cloud_floor->width = 0;
    point_cloud_floor->header.seq =g_seq;

    point_cloud_nofloor->header.frame_id = "velodyne";
    point_cloud_nofloor->height = 1;
    point_cloud_nofloor->header.stamp = dt.currentMSecsSinceEpoch();
    point_cloud_nofloor->width = 0;
    point_cloud_nofloor->header.seq =g_seq;

    g_seq++;


    int i,j;
    for(i=0;i<(nVecSize+2);i++)
    {
        for(j=0;j<(nHorSize+2);j++)
        {
            floor_find[i*(nHorSize+2) +j] = false;
        }
    }

    for(i=0;i<nVecSize;i++)
    {
        for(j=0;j<nHorSize;j++)
        {
            floor_h[i*nHorSize+j] = -SENSOR_HEIGHT;
        }
    }

    for(i=0;i<point_cloud->size();i++)
    {
        float x = point_cloud->at(i).x;
        float y = point_cloud->at(i).y;
        float z = point_cloud->at(i).z;

        if((x>=skip_xmin)&&(y>=skip_ymin)&&(x<=skip_xmax)&&(y<=skip_ymax))
        {
            continue;
        }
        float dis = sqrt(x*x + y*y);

//        if((dis>0.9)&&(z>(-SENSOR_HEIGHT-dis*General_Thresh_Ratio))&&(z<(-SENSOR_HEIGHT+dis*General_Thresh_Ratio)))
        if((z>(-SENSOR_HEIGHT-dis*General_Thresh_Ratio))&&(z<(-SENSOR_HEIGHT+dis*General_Thresh_Ratio)))
        {
            int Rpos,Cpos;
            Rpos = (y-VEC_MIN)/GRID_SIZE;
            Cpos = (x-HOR_MIN)/GRID_SIZE;
            if((Rpos>=0)&&(Rpos<nVecSize)&&(Cpos>=0)&&(Cpos<nHorSize))
            {
                int xPos = Rpos*(nHorSize+2) + Cpos+1;
                if(floor_find[xPos] == true)
                {
                    if(z<floor_h_raw[xPos])
                    {
                        floor_h_raw[xPos] = z;
                    }
                }
                else
                {
                    floor_find[xPos]= true;
                    floor_h_raw[xPos] = z;
                }
            }
        }
    }

    for(i=1;i<(nVecSize+1);i++)
    {
        for(j=1;j<(nHorSize+1);j++)
        {
            int m,n;
            bool bHave = false;
            float min;
            for(m=-1;m<=1;m++)
            {
                for(n=-1;n<=1;n++)
                {
                    int xpos = (i+m)*(nHorSize+2) + j+n;
                    if(floor_find[xpos])
                    {
                        if(bHave)
                        {
                            if(min<floor_h_raw[xpos])min = floor_h_raw[xpos];
                        }
                        else
                        {
                            min = floor_h_raw[xpos];
                            bHave = true;
                        }
                    }
                }
            }
            if(bHave)floor_h[(i-1)*nHorSize +j] = min;
        }
    }

//#ifdef OUT_GROUND

    for(i=0;i<point_cloud->size();i++)
    {
        pcl::PointXYZI po = point_cloud->at(i);
        float x = point_cloud->at(i).x;
        float y = point_cloud->at(i).y;
        float z = point_cloud->at(i).z;
        if((x>=skip_xmin)&&(y>=skip_ymin)&&(x<=skip_xmax)&&(y<=skip_ymax))
        {
            continue;
        }
//        float dis = sqrt(x*x + y*y);
        int Rpos,Cpos;
        Rpos = (y-VEC_MIN)/GRID_SIZE;
        Cpos = (x-HOR_MIN)/GRID_SIZE;
        if((Rpos>=0)&&(Rpos<nVecSize)&&(Cpos>=0)&&(Cpos<nHorSize))
        {
            if((z-floor_h[Rpos*nHorSize+Cpos])>0.15)
            {
               point_cloud_nofloor->push_back(po);
               ++point_cloud_nofloor->width;
            }
            else
            {
                point_cloud_floor->push_back(po);
                ++point_cloud_floor->width;
            }

        }
    }

//#endif

//    for(i=0;i<nVecSize;i++)
//    {
//        char strOut[3000];
//        char strTem[30];
//        snprintf(strOut,3000,"i = %d",i);
//        for(j=0;j<nHorSize;j++)
//        {
//          //  floor_h[i*nHorSize+j] = -SENSOR_HEIGHT;
//            snprintf(strTem,30,"%6.3f ",floor_h[i*nHorSize+j]);
//            strncat(strOut,strTem,3000);
//        }
//        qDebug(strOut);
//    }

#ifdef OUT_GROUND
    writepctosm(point_cloud_floor,g_lidar_pc_floor);
    writepctosm(point_cloud_nofloor,g_lidar_pc_nofloor);
#endif

    int NYC = VEC_LEN/SMALLGRID_SIZE;
    int NXC = HOR_LEN/SMALLGRID_SIZE;
    iv::LidarGrid * pobsgrid = new iv::LidarGrid[NYC*NXC];//from 50m back 50m left 25m right 25m
//    int i,j;
    for(i=0;i<NYC;i++)
    {
        for(j=0;j<NXC;j++)
        {
            iv::LidarGrid * p = pobsgrid + i*NXC+j;
            p->pointcount = 0;
            int Rpos,Cpos;
            Rpos = (i*SMALLGRID_SIZE)/GRID_SIZE;
            Cpos = (j*SMALLGRID_SIZE)/GRID_SIZE;
            if((Rpos>=0)&&(Rpos<nVecSize)&&(Cpos>=0)&&(Cpos<nHorSize))
            {
                int xPos = Rpos*nHorSize + Cpos;
                p->groundheight = floor_h[xPos];
            }
            else
            {
                p->groundheight = -SENSOR_HEIGHT+1000;
            }
            p->low = -SENSOR_HEIGHT+1000;
            p->high = -SENSOR_HEIGHT - 1000;
            p->obshight = 0;
            p->ob = 0;
        }
    }

    for(i=0;i<point_cloud_nofloor->size();i++)
    {
        pcl::PointXYZI pxyzi = point_cloud_nofloor->at(i);
        int x,y;
        x = (int)((pxyzi.y-VEC_MIN)/SMALLGRID_SIZE);
        y = (int)((pxyzi.x-HOR_MIN)/SMALLGRID_SIZE);
        if((x>=0)&&(x<NXC)&&(y<NYC)&&(y>=0))
        {
                iv::LidarGrid * p = pobsgrid + x*NXC+y;
                p->ob++;
                if(pxyzi.z<p->low)p->low = pxyzi.z;
                if(pxyzi.z>p->high)p->high = pxyzi.z;
        }
    }

    for(i=0;i<NYC;i++)
    {
        for(j=0;j<NXC;j++)
        {
            iv::LidarGrid * p = pobsgrid + i*NXC+j;
            if(p->ob>=3)
            {
                if(p->low>(p->groundheight+VEHICLE_HEIGHT))
                {
   //                 std::cout<<"low high"<<std::endl;
                }
                else
                {
//                    temp.nomal_x = (j-125)*0.2+0.1;
//                    temp.nomal_y = (i-250)*0.2+0.1;


                    iv::ObstacleBasic temp;
                    temp.low = p->low;
                    temp.high = p->high;
                    temp.nomal_z = p->high-p->low;
                    temp.nomal_x = j*SMALLGRID_SIZE + 0.1+HOR_MIN;
                    temp.nomal_y = i*SMALLGRID_SIZE +0.1 +VEC_MIN;
                    lidar_obs->push_back(temp);
                }
            }
        }
    }

    delete pobsgrid;

    delete floor_find;
    delete floor_h_raw;
    delete floor_h;

}





#include <QTime>

#include "fusionobjectarray.pb.h"

iv::fusion::fusionobjectarray xfusion;

void ShareFusionObject(void * pa ,std::shared_ptr<std::vector<iv::ObstacleBasic>> & lidar_obs)
{

    double timestamp = std::chrono::system_clock::now().time_since_epoch().count();
    timestamp = timestamp/1000000.0;

    xfusion.clear_obj();

    xfusion.set_timestamp(timestamp);

    iv::fusion::fusionobject * pobj =  xfusion.add_obj();

    pobj->set_id(0);
    pobj->set_type(0);
    iv::fusion::VelXY *pxvel = new iv::fusion::VelXY;
    pxvel->set_x(0);
    pxvel->set_y(0);
    pobj->set_allocated_vel_abs(pxvel);
    iv::fusion::Dimension * pxdim = new iv::fusion::Dimension;
    pxdim->set_x(0.2);
    pxdim->set_y(0.2);
    pxdim->set_z(2.0);

    pobj->set_allocated_dimensions(pxdim);

    int i;
    int nsize = lidar_obs->size();
    for(i=0;i<nsize;i++)
    {
        iv::fusion::NomalXYZ * pcen = pobj->add_nomal_centroid();
        pcen->set_nomal_x(lidar_obs->at(i).nomal_x);
        pcen->set_nomal_y(lidar_obs->at(i).nomal_y);
        pcen->set_nomal_z(lidar_obs->at(i).nomal_z);
    }

    int nbytesize = xfusion.ByteSize();
    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);

    if(xfusion.SerializeToArray(pstr_ptr.get(),nbytesize))
    {
        iv::modulecomm::ModuleSendMsg(pa,pstr_ptr.get(),nbytesize);
    }
    else
    {
        std::cout<<" serialzie fusion fail."<<std::endl;
    }


}


static void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

//    std::cout<<"enter listen."<<std::endl;
    if(nSize <=16)return;
    int xin = index;
    xin++;
    unsigned int * pHeadSize = (unsigned int *)strdata;
    if(*pHeadSize > nSize)
    {
        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());
    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char*)strdata +4),nNameSize);
    point_cloud->header.frame_id = strName;
    memcpy(&point_cloud->header.seq,((char*)strdata)+4+nNameSize,4);
    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)((char*)strdata + *pHeadSize);
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZI xp;
        xp.x = p->x;
        xp.y = p->y;
        xp.z = p->z;
        xp.intensity = p->intensity;
        point_cloud->push_back(xp);
        p++;
    }

    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);

    GridGetObs(point_cloud,lidar_obs);
//    perception_lidar_vv7_ProcObs(point_cloud,lidar_obs);

//   std::cout<<"prc."<<std::endl;

    ShareFusionObject(gpafusion,lidar_obs);

    if(g_robosys == 0)
    {
        if(lidar_obs->size() == 0)
        {
            iv::ObstacleBasic temp;
            temp.low = 0;
            temp.high = 3.0;
            temp.nomal_z = 3.0;
            temp.nomal_x = 1000;
            temp.nomal_y = 1000;
            lidar_obs->push_back(temp);
        }
        if(lidar_obs->size() >0)iv::modulecomm::ModuleSendMsg(glidar_obs,(char *)(lidar_obs->data()),lidar_obs->size()*sizeof(iv::ObstacleBasic));
    }
    else
    {
        g_robosys--;
        if(g_robosys<0)
        {
            g_robosys = 0;
        }
    }
    gfault->SetFaultState(0, 0, "ok");
 //   return;
//    gw->UpdatePointCloud(point_cloud);


}




void  PERCEPTION_LIDAR_VV7SHARED_EXPORT StartPerception_lidar_vv7()
{
    std::cout<<"HHINow Start LIDAR Perception"<<std::endl;

    glidar_obs = iv::modulecomm::RegisterSend(gstr_outputmemname,20000000,3);

    gpafusion = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);

#ifdef OUT_GROUND
    g_lidar_pc_floor = iv::modulecomm::RegisterSend("lidar_pc_floor",20000000,3);
    g_lidar_pc_nofloor = iv::modulecomm::RegisterSend("lidar_pc_nofloor",20000000,3);
#endif
    glidar_pc =  iv::modulecomm::RegisterRecv(gstr_inputmemname,ListenPointCloud);


}

void PERCEPTION_LIDAR_VV7SHARED_EXPORT StopPerception_lidar_vv7()
{




}



void decodeyaml(const char * stryaml)
{
    YAML::Node config;
    try
    {
        config = YAML::LoadFile(stryaml);
    }
    catch(YAML::BadFile e)
    {
        givlog->verbose("load yaml error.");
        return;
    }

    if(config["SensorHeight"])
    {
        strncpy(gstr_sensorheight,config["SensorHeight"].as<std::string>().data(),255);
    }

    if(config["VehicleHeight"])
    {
        strncpy(gstr_vehicleheight,config["VehicleHeight"].as<std::string>().data(),255);
    }

    if(config["inputmemname"])
    {
        strncpy(gstr_inputmemname,config["inputmemname"].as<std::string>().data(),255);
    }

    if(config["outputmemname"])
    {
        strncpy(gstr_outputmemname,config["outputmemname"].as<std::string>().data(),255);
    }

    if(config["skip_xmin"])
    {
        strncpy(gstr_skipxmin,config["skip_xmin"].as<std::string>().data(),255);
    }

    if(config["skip_xmax"])
    {
        strncpy(gstr_skipxmax,config["skip_xmax"].as<std::string>().data(),255);
    }

    if(config["skip_ymin"])
    {
        strncpy(gstr_skipymin,config["skip_ymin"].as<std::string>().data(),255);
    }

    if(config["skip_ymax"])
    {
        strncpy(gstr_skipymax,config["skip_ymax"].as<std::string>().data(),255);
    }


//    std::cout<<gstr_memname<<std::endl;
//    std::cout<<gstr_rollang<<std::endl;
//    std::cout<<gstr_inclinationang_xaxis<<std::endl;
//    std::cout<<gstr_inclinationang_yaxis<<std::endl;
//    std::cout<<gstr_hostip<<std::endl;
//    std::cout<<gstr_port<<std::endl;
}


char gstr_yaml[256];


void print_useage()
{
    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<std::endl;
    std::cout<<" -h --help print help"<<std::endl;
}

int  GetOptLong(int argc, char *argv[]) {
    int nRtn = 0;
    int opt; // getopt_long() 的返回值
    int digit_optind = 0; // 设置短参数类型及是否需要参数

    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
    // 第几个元素的描述，即是long_opts的下标值
    int option_index = 0;
    // 设置短参数类型及是否需要参数
    const char *optstring = "s:h";

    // 设置长参数类型及其简写，比如 --reqarg <==>-r
    /*
    struct option {
             const char * name;  // 参数的名称
             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
        };
    其中：
        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
     */
    static struct option long_options[] = {
        {"setyaml", required_argument, NULL, 's'},
        {"help",  no_argument,       NULL, 'h'},
 //       {"optarg", optional_argument, NULL, 'o'},
        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
    };

    while ( (opt = getopt_long(argc,
                               argv,
                               optstring,
                               long_options,
                               &option_index)) != -1) {
//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
//        printf("optarg = %s\n", optarg); // 参数内容
//        printf("optind = %d\n", optind); // 下一个被处理的下标值
//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
//        printf("\n");
        switch(opt)
        {
        case 's':
            strncpy(gstr_yaml,optarg,255);
            break;
        case 'h':
            print_useage();
            nRtn = 1; //because use -h
            break;
        default:
            break;
        }

    }

    return nRtn;
}


int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("detection_lidar_grid");
    QCoreApplication a(argc, argv);
    gfault = new iv::Ivfault("lidar_grid");
    givlog = new iv::Ivlog("lidar_grid");

    snprintf(gstr_yaml,255,"");

    int nRtn = GetOptLong(argc,argv);
    if(nRtn == 1)  //show help,so exit.
    {
        return 0;
    }

    snprintf(gstr_sensorheight,255,"2.0");
    snprintf(gstr_vehicleheight,255,"2.3");
    snprintf(gstr_inputmemname,255,"lidar_pc");
    snprintf(gstr_outputmemname,255,"lidar_obs");
    snprintf(gstr_skipxmin,255,"-0.9");
    snprintf(gstr_skipxmax,255,"0.9");
    snprintf(gstr_skipymin,255,"-2.3");
    snprintf(gstr_skipymax,255,"2.3");


    givlog->verbose("yaml is %s ",gstr_yaml);

    if(strnlen(gstr_yaml,255)>0)
    {
        decodeyaml(gstr_yaml);
    }

    SENSOR_HEIGHT = atof(gstr_sensorheight);
    VEHICLE_HEIGHT = atof(gstr_vehicleheight);
    skip_xmin = atof(gstr_skipxmin);
    skip_xmax = atof(gstr_skipxmax);
    skip_ymin = atof(gstr_skipymin);
    skip_ymax = atof(gstr_skipymax);

    StartPerception_lidar_vv7();

    return a.exec();
}