雷达融合笔记及一些易错点总结（1）----------一线激光雷达

一线激光雷达程序流程图如下：（本地测试用）

.h文件

#include"stdafx.h"
#include<iostream>
#include<vector>
#include<math.h>
#include <Winsock2.h> //window.h必须放在Winsock2.h的下面
#include<windows.h>
#include<Winbase.h>
#pragma comment(lib,"WS2_32.lib")
using namespace std;

typedef unsigned short int  uint;
typedef unsigned char uchar;


#define AngleMin  10          //角度筛选 10  310
#define AngleMax  370
#define RaderYwide   30000      //最远看30m
#define RaderXwide 10000     //雷达有效范围20m。  最大支持100*160*4 byte = 64K

#define  MyPort 5080   // 本机端口号
//#define  MyPort 8060   // 本机端口号

#define MaxSize 100


#define Forward 300
#define LeftRight 100

#define MAXSIZE 381     // scan data模式  获得的最多点的个数1680（一定要清楚其范围，1680*6=）

int m_XYDist[2][381];
UINT32 m_DIST[381];
typedef struct RaderInfor
{   //加 # 是为了保证数据正确，相当于一个校验的作用。
    unsigned int dist[381];
} RaderInfor;

RaderInfor raderinfor;


typedef struct Point        //点转换为矩阵的x与y坐标。
{
    uint x;
    uint y;
    uchar value;
    uchar U;
} Point;
typedef struct Matrix      //稀疏矩阵数据结构
{
    int Num;
    Point point[MAXSIZE]; //

} Matrix;

Matrix matrix;             //稀疏矩阵的声明，注：此处并未初始化

int m = 0;  //刚刚定义为局部变量，老是出错。
int n = 0;

.cpp文件

// ReadFileData.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include"SendMsg.h"


int _tmain(int argc, _TCHAR* argv[])
{
    //UDP套接字初始化
    WSADATA wsaData;
    SOCKET sClient;
    int iLen;  //服务器地址长度
    int iSend; //接收数据的缓冲
    struct sockaddr_in ser; //服务器端地址 // IPv4 Socket address, Internet style
    if(WSAStartup(MAKEWORD(2,2),&wsaData) !=0 ) //套接字初始化
    {
        printf("Failed to load Winsock.
");
        return false;
    }
    /***********建立服务器端地址***************/
    ser.sin_family = AF_INET;
    ser.sin_port = htons(MyPort);
    ser.sin_addr.s_addr=inet_addr("127.0.0.1"); // 本机IP地址，测试用
    //ser.sin_addr.s_addr=inet_addr("192.168.1.113");  //决策机IP地址
    sClient = socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP);//建立客户端数据报套接字接口。
    //至此，套接字将两端的对象封装完毕，只等发数据了。
    if(sClient == INVALID_SOCKET)
    {
        printf("socket()Failed: %d 
",WSAGetLastError());
        return false;
    }
    iLen = sizeof(ser);

    while(1)
    {

    FILE *infile = fopen("data.txt","rb");

    long i = 0;

    while(!feof(infile))
    {
        Sleep(200);
        memset(&raderinfor,0,sizeof(RaderInfor));
        //文件的随机读取 我是一个结构体，一个结构体的去存取数据的
        fseek(infile,i*sizeof(RaderInfor),SEEK_SET);
        ++i;
        fread(&raderinfor,sizeof(RaderInfor),1,infile);
        //将结构体内的数据读取出来并赋值给m_DIST数组
        for (int i = 0;i < 381; ++i)
        {
            if (abs(raderinfor.dist[i]*1.0) < 30000)
            {
                m_DIST[i] = raderinfor.dist[i];
            }
            else
            {
                m_DIST[i] = 60000;
            }
        }
        //转换为直角坐标
        for(int i=0; i<381; ++i)
        {
            float theta = (i*0.5-5) / 180 * 3.1415; //角度换算。380/0.5=190;前5°要了。
            float x = cos( theta ) * m_DIST[i];     //也就是说，m_DIST[i]为R。
            float y = sin( theta ) * m_DIST[i];

            m_XYDist[0][i] = x;
            m_XYDist[1][i] = y;
        }

                  //数据处理部分
              int m = 0,n = 0;
              int obs = 0;

              memset(&matrix,0,sizeof(matrix));

              for(int i = AngleMin;i < AngleMax ;++i )  //记录下20米内的障碍物信息，包括距离、坐标、障碍物角度。
              {
                  if(((m_XYDist[1][i] > 0) && (m_XYDist[1][i] < RaderYwide))&&(abs(m_XYDist[0][i])< RaderXwide))
                  {
                      m = m_XYDist[0][i]/200;
                      m = m + LeftRight/2;
                      //  m = m + LeftRight/2 - 1;   //坐标变换公式，通过变换使得以左上角为原点的坐标。
                      n = m_XYDist[1][i]/200;
                      n = Forward/2 - n;
                      //    n = Forward/2 - 1 - n;
                      if((m<100&&m>0)&&(n<300&&n>0))
                      {
                          matrix.point[obs].x = n;
                          matrix.point[obs].y = m;
                          matrix.point[obs].value = 128;
                          matrix.point[obs].U = 0;
                          obs++;
                      }
                  }
              }
              matrix.Num = obs;


        /****************************   数据处理程序  ******************************/
             if(matrix.Num > 0)
           {
        //******************************UDP发送之前，将结构体转换为字符数组******************************/
                 //uint len = (matrix.Num)*sizeof(Point) + 4;
                 //vector<char> str(len);       //声明可变数组  ,  容器已经初始化过了。
                 //memcpy((char*)&str[0],(char*)&matrix,str.size());//从结构体matrix里面截取部分长度，并赋值给str数组。str.size()  len
              //   iSend = sendto(sClient,(char*)&str[0],len-1,0,(struct sockaddr*)&ser,iLen);
               uint len = (obs)*sizeof(Point) + 4;
               char buffer[10000];
               memcpy(buffer,(char*)&matrix,len);
               iSend=sendto(sClient,buffer,len-1,0,(struct sockaddr*)&ser,iLen);

                 if(iSend == SOCKET_ERROR)
                     cout << "发送错误" << endl;
                 cout << "ok！" << endl;
          }//if判断障碍物个数大于0结束
    }    //while循环结束
    fclose(infile);
    }
    return 0;
}

易错点：

1、在转换为字节流的时候，注意开辟足够的空间。

2、在转换为栅格图的时候，注意边界。

3、调试的时候，当我们想要知道数据有没有发送出去的时候，直接看iSend返回值即可。