ROS开发之激光雷达数据的解析与应用（自动驾驶，无人船，无人机等等避障）

ROS开发之激光雷达数据的解析与应用（自动驾驶，无人船，无人机等等避障）

一、激光雷达工作原理及数据解析

1.常用测距方式

        对于市面上的主流激光雷达，主要是用于环境探测、地图构建，按技术路线可分为： 三角测距 激光雷达， TOF 激光雷

2.以某个TOF激光雷达为例

目前激光雷达工作模式基本都是上电，启动测量，输出数据，这样一个三部曲（有的雷达上电后可以直接输出测量数据，有的雷达需要通过驱动下发指令来启动，所以仅仅通电不做其他操作存在上电雷达直接转动和上电不转动两种区别）

知道工作模式后接下来关注便是如何使用激光雷达，一般通过给定的驱动或者直接通电就可以直接使用。在通过基本的使用操作顺利进行后就可以接收来自雷达发出的数据进行利用，目前基本都是雷达通过串口方式向外发送测完的距离数据信息。而发送出来的信息一般都是由给定的协议向外发送。

如图1一般是商家给的开发手册，2D激光协议方式基本大同小异，我们要利用激光传输出来的数据，只需要对应解析数据即可

                                                                      图1 激光雷达数据协议

二、数据接收及数据处理

1.硬件接收处理

  此处谈到硬件接收处理便是硬件之间的电气特性要匹配，比如串口通要区分是TTL、232、485、CAN等等不同硬件通信方案。一般厂家提供的usart输出及为TTL输出，有TTL转USB数据线或者模块可以直接连接电脑使用，当然涉及嵌入式硬件比如JetsonNano、树莓派、工控上位机等等也是一样的道理，区分硬件匹配搭配相应通信设备即可。具体事例更新中。。。

2.软件接收处理

软件处理便是在满足硬件设备能互相通信后进行匹配商家传输协议进行速度信息、时间信息、角度方向等信息进去解析读取便于我们开发利用。具体事例更新中。。。

三、ROS中数据的发布与利用（建图，导航避障）

1.建图

2.避障

2.1避障初步数据处理

void ObstacleAvoidance360(void)//x,y,no have z 
{
	
float angle360[360]={};//float degree = RAD2DEG( lidar2Dscan->angle_min + lidar2Dscan->angle_increment * i);i=360 //Replace global variables here
float	currentLidarDistanceThresholdMax360=1;
float currentLidarDistanceThresholdMin360=1;
float currentLidarDistance360[360]={};//lidar2Dscan->ranges[i]  //Replace global variables here
float currentLidarDistanceThresholdMax360=2;
float currentLidarDistanceThresholdMin360=1;	
float DistanceY=0;
float Distancex=0;
float RatioY[360]={};
float RatioX[360]={};	
uint8_t	currentLidarDistanceThresholdMin360TriggerCount[360]={};//Replace global variables here
uint8_t currentLidarDistanceThresholdMax360TriggerCount[360]={};//Replace global variables here
#define currentLidarDistanceThresholdMin360TriggerCountNumThreshold 20
#define currentLidarDistanceThresholdMax360TriggerCountNumThreshold 20
#define setPosePositionBase_x 0.1//Special resolution for obstacle avoidance
#define setPosePositionbase_y 0.1
#define setPosePositionbase_z 0.1
	for(i=0;i<360;i++)//add  lidardistance and angle recevie status flag
	{
		currentLidarDistance360[i]=lidar2Dscan->ranges[i];
		angle360[i]=RAD2DEG( lidar2Dscan->angle_min + lidar2Dscan->angle_increment * i);
		if(currentLidarDistance360[i]<currentLidarDistanceThresholdMin360) //min detection
    {
	       currentLidarDistanceThresholdMin360TriggerCount[i]+=1;
				if(currentLidarDistanceThresholdMin360TriggerCount[i]>currentLidarDistanceThresholdMin360TriggerCountNumThreshold)
		{
			
			if(angle360[i]>0&&angle360<90){//headleft   move x->back   y->right
		        	RatioY[i] = (currentLidarDistance360[i]*sin(angle360))/(currentLidarDistance360[i]*sin(angle360)+currentLidarDistance360[i]*cos(angle360));
	            RatioX[i] = (currentLidarDistance360[i]*cos(angle360))/(currentLidarDistance360[i]*sin(angle360)+currentLidarDistance360[i]*cos(angle360));
				      pose.pose.position.x -= RatioX[i]*setPosePositionBase_x; 
				      pose.pose.position.y -= RatioY[i]*setPosePositionBase_y;   				
				
		   	}
			else if(angle360[i]>90&&angle360<180){//backlleft  move x->head   y->right
		        	RatioY[i] = (currentLidarDistance360[i]*sin(angle360-90))/(currentLidarDistance360[i]*sin(angle360-90)+currentLidarDistance360[i]*cos(angle360-90));
	            RatioX[i] = (currentLidarDistance360[i]*cos(angle360-90))/(currentLidarDistance360[i]*sin(angle360-90)+currentLidarDistance360[i]*cos(angle360-90));
							pose.pose.position.x += RatioX[i]*setPosePositionBase_x; 
				      pose.pose.position.y -= RatioY[i]*setPosePositionBase_y;
				
				
		  	}
			else if(angle360[i]>180&&angle360<270){//br move x->head   y->left
			        RatioY[i] = (currentLidarDistance360[i]*sin(angle360-180))/(currentLidarDistance360[i]*sin(angle360-180)+currentLidarDistance360[i]*cos(angle360-180));
	            RatioX[i] = (currentLidarDistance360[i]*cos(angle360-180))/(currentLidarDistance360[i]*sin(angle360-180)+currentLidarDistance360[i]*cos(angle360-180));
				      pose.pose.position.x += RatioX[i]*setPosePositionBase_x; 
				      pose.pose.position.y += RatioY[i]*setPosePositionBase_y;
				
	   		}
			else {//hr  move x->back   y->left  move x->back   y->left
					   	RatioY[i] = (currentLidarDistance360[i]*sin(angle360-360))/(currentLidarDistance360[i]*sin(angle360-360)+currentLidarDistance360[i]*cos(angle360-360));
	            RatioX[i] = (currentLidarDistance360[i]*cos(angle360-360))/(currentLidarDistance360[i]*sin(angle360-360)+currentLidarDistance360[i]*cos(angle360-360));
				      pose.pose.position.x -= RatioX[i]*setPosePositionBase_x; 
				      pose.pose.position.y += RatioY[i]*setPosePositionBase_y;
				
		      	} 
		     for(i=0;i<360;i++){ currentLidarDistanceThresholdMin360TriggerCount[i]=0;}	 
			}
    }	
		
	 if(currentLidarDistance360[i]>currentLidarDistanceThresholdMax360)//max detection
		{
			    currentLidarDistanceThresholdMax360TriggerCount[i]+=1;
					if(currentLidarDistanceThresholdMax360TriggerCount>currentLidarDistanceThresholdMax360TriggerCountNumThreshold)
			{
				  for(i=0;i<360;i++){ currentLidarDistanceThresholdMax360TriggerCount[i]=0;currentLidarDistance360[i]=0;}
			}
		}	
	}
	
	
	for(i=0;i<360;i++){currentLidarDistance360[i]=0;}  //Complete a round of calculation
		
}

持续更新中。。。。。。