单线激光雷达Windows测试备忘

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

.h文件

   1:   

   2:  #include"stdafx.h"

   3:  #include<iostream>

   4:  #include<vector>

   5:  #include<math.h>

   6:  #include <Winsock2.h> //window.h必须放在Winsock2.h的下面

   7:  #include<windows.h>

   8:  #include<Winbase.h>

   9:  #pragma comment(lib,"WS2_32.lib")

  10:  using namespace std;

  11:   

  12:  typedef unsigned short int  uint;

  13:  typedef unsigned char uchar;

  14:   

  15:   

  16:  #define AngleMin  10          //角度筛选 10  310 

  17:  #define AngleMax  370    

  18:  #define RaderYwide   30000      //最远看30m

  19:  #define RaderXwide 10000     //雷达有效范围20m。  最大支持100*160*4 byte = 64K 

  20:   

  21:  #define  MyPort 5080   // 本机端口号

  22:  //#define  MyPort 8060   // 本机端口号

  23:   

  24:  #define MaxSize 100

  25:   

  26:   

  27:  #define Forward 300    

  28:  #define LeftRight 100

  29:   

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

  31:   

  32:  int m_XYDist[2][381];

  33:  UINT32 m_DIST[381];

  34:  typedef struct RaderInfor

  35:  {   //加 # 是为了保证数据正确，相当于一个校验的作用。

  36:      unsigned int dist[381];

  37:  } RaderInfor;

  38:   

  39:  RaderInfor raderinfor;

  40:   

  41:   

  42:  typedef struct Point        //点转换为矩阵的x与y坐标。

  43:  {

  44:      uint x;

  45:      uint y;

  46:      uchar value;

  47:      uchar U;

  48:  } Point;

  49:  typedef struct Matrix      //稀疏矩阵数据结构

  50:  {

  51:      int Num;

  52:      Point point[MAXSIZE]; //

  53:   

  54:  } Matrix;

  55:   

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

  57:   

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

  59:  int n = 0;

  60:   

  61:   

  62:   

  63:   

  64:   

  65:   

  66:   

  67:   

  68:   

  69:   

  70:   

  71:   

.cpp文件

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

   2:  //

   3:   

   4:  #include "stdafx.h"

   5:  #include"SendMsg.h"

   6:   

   7:   

   8:  int _tmain(int argc, _TCHAR* argv[])

   9:  {

  10:      //UDP套接字初始化 

  11:      WSADATA wsaData;

  12:      SOCKET sClient;

  13:      int iLen;  //服务器地址长度

  14:      int iSend; //接收数据的缓冲

  15:      struct sockaddr_in ser; //服务器端地址 // IPv4 Socket address, Internet style

  16:      if(WSAStartup(MAKEWORD(2,2),&wsaData) !=0 ) //套接字初始化

  17:      {

  18:          printf("Failed to load Winsock.\n");

  19:          return false;

  20:      }

  21:      /***********建立服务器端地址***************/

  22:      ser.sin_family = AF_INET;

  23:      ser.sin_port = htons(MyPort);

  24:      ser.sin_addr.s_addr=inet_addr("127.0.0.1"); // 本机IP地址，测试用

  25:      //ser.sin_addr.s_addr=inet_addr("192.168.1.113");  //决策机IP地址

  26:      sClient = socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP);//建立客户端数据报套接字接口。

  27:      //至此，套接字将两端的对象封装完毕，只等发数据了。

  28:      if(sClient == INVALID_SOCKET)

  29:      {

  30:          printf("socket()Failed: %d \n",WSAGetLastError());

  31:          return false;

  32:      }

  33:      iLen = sizeof(ser);

  34:   

  35:      while(1)

  36:      {

  37:   

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

  39:   

  40:      long i = 0;

  41:   

  42:      while(!feof(infile))

  43:      {

  44:          Sleep(200);

  45:          memset(&raderinfor,0,sizeof(RaderInfor));

  46:          //文件的随机读取 我是一个结构体，一个结构体的去存取数据的

  47:          fseek(infile,i*sizeof(RaderInfor),SEEK_SET);

  48:          ++i;

  49:          fread(&raderinfor,sizeof(RaderInfor),1,infile);

  50:          //将结构体内的数据读取出来并赋值给m_DIST数组

  51:          for (int i = 0;i < 381; ++i)

  52:          {

  53:              if (abs(raderinfor.dist[i]*1.0) < 30000)

  54:              {

  55:                  m_DIST[i] = raderinfor.dist[i];

  56:              }

  57:              else

  58:              {

  59:                  m_DIST[i] = 60000;

  60:              }

  61:          }

  62:          //转换为直角坐标

  63:          for(int i=0; i<381; ++i)

  64:          {

  65:              float theta = (i*0.5-5) / 180 * 3.1415; //角度换算。380/0.5=190;前5°要了。

  66:              float x = cos( theta ) * m_DIST[i];     //也就是说，m_DIST[i]为R。

  67:              float y = sin( theta ) * m_DIST[i];

  68:   

  69:              m_XYDist[0][i] = x;

  70:              m_XYDist[1][i] = y;

  71:          }

  72:   

  73:                    //数据处理部分

  74:                int m = 0,n = 0;

  75:                int obs = 0;

  76:   

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

  78:   

  79:                for(int i = AngleMin;i < AngleMax ;++i )  //记录下20米内的障碍物信息，包括距离、坐标、障碍物角度。

  80:                {

  81:                    if(((m_XYDist[1][i] > 0) && (m_XYDist[1][i] < RaderYwide))&&(abs(m_XYDist[0][i])< RaderXwide))

  82:                    {

  83:                        m = m_XYDist[0][i]/200;  

  84:                        m = m + LeftRight/2; 

  85:                        //  m = m + LeftRight/2 - 1;   //坐标变换公式，通过变换使得以左上角为原点的坐标。

  86:                        n = m_XYDist[1][i]/200;

  87:                        n = Forward/2 - n; 

  88:                        //    n = Forward/2 - 1 - n; 

  89:                        if((m<100&&m>0)&&(n<300&&n>0))

  90:                        {

  91:                            matrix.point[obs].x = n;

  92:                            matrix.point[obs].y = m;

  93:                            matrix.point[obs].value = 128;

  94:                            matrix.point[obs].U = 0;

  95:                            obs++;

  96:                        }

  97:                    }

  98:                }

  99:                matrix.Num = obs;

 100:   

 101:         

 102:          /****************************   数据处理程序  ******************************/

 103:               if(matrix.Num > 0)

 104:             { 

 105:          //******************************UDP发送之前，将结构体转换为字符数组******************************/    

 106:                   //uint len = (matrix.Num)*sizeof(Point) + 4; 

 107:                   //vector<char> str(len);       //声明可变数组  ,  容器已经初始化过了。

 108:                   //memcpy((char*)&str[0],(char*)&matrix,str.size());//从结构体matrix里面截取部分长度，并赋值给str数组。str.size()  len

 109:                //   iSend = sendto(sClient,(char*)&str[0],len-1,0,(struct sockaddr*)&ser,iLen);

 110:                 uint len = (obs)*sizeof(Point) + 4; 

 111:                 char buffer[10000];

 112:                 memcpy(buffer,(char*)&matrix,len);

 113:                 iSend=sendto(sClient,buffer,len-1,0,(struct sockaddr*)&ser,iLen);

 114:                   

 115:                   if(iSend == SOCKET_ERROR)

 116:                       cout << "发送错误" << endl;

 117:                   cout << "ok！" << endl;

 118:            }//if判断障碍物个数大于0结束    

 119:      }    //while循环结束

 120:      fclose(infile);

 121:      }

 122:      return 0;

 123:  }

 124:   

易错点：

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

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

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