雷达融合笔记及一些易错点总结（1）----------一线激光雷达
一线激光雷达程序流程图如下：（本地测试用）
.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返回值即可。
