完整代码+A库标定参数

build/setting/setting.prototxt

@@ -0,0 +1,123 @@
+plc_setting
+{	
+	ip:"192.168.0.81"
+	dbnumber:9050
+	start_id:0
+}
+#入口
+entrance_parameter
+{
+	lidars
+	{
+		
+		sncode:"3GGDJ8S00100921"
+		r:175.69
+		p:24.49
+		y:-83.43
+		tx:-1.2322
+		ty:0
+		tz:0
+		fps:5.0
+	}
+	lidars
+	{
+		
+		sncode:"3GGDJ8X00100541"
+		r:173.87
+		p:27.51
+		y:-98.23
+		tx:1.2677
+		ty:0.0109
+		tz:-0.013
+		fps:5.0
+	}
+	
+	box
+	{
+		minx:-1.7
+		maxx:1.7
+		miny:-4.95
+		maxy:-0.18
+		minz:-1.59
+		maxz:0.4
+	}
+	verify_box1
+	{
+		minx:-1.20
+		maxx:-0.992
+		miny:-5.31
+		maxy:-0.18
+		minz:-1.54
+		maxz:-0.1
+	}
+	verify_box2
+	{
+		minx:0.982
+		maxx:1.192
+		miny:-5.31
+		maxy:-0.33
+		minz:-1.54
+		maxz:-0.1
+	}
+}
+
+
+export_parameter
+{
+	lidars
+	{
+		
+		sncode:"3GGDJ8T00100171"
+		r:-169.23
+		p:27.71
+		y:81.1
+		tx:-1.17415
+		ty:0.018
+		tz:0.0196
+		fps:5.0
+	}
+	lidars
+	{
+		
+		sncode:"3GGDJ8X00100481"
+		r:-176.71
+		p:22.5
+		y:100.82
+		tx:1.23285
+		ty:0
+		tz:0
+		fps:5.0
+	}
+	
+	box
+	{
+		minx:-1.7
+		maxx:1.7
+		miny:0.
+		maxy:6
+		minz:-1.6
+		maxz:0.4
+	}
+	verify_box1
+	{
+		minx:-1.20
+		maxx:-0.992
+		miny:0.15
+		maxy:4.91
+		minz:-1.53
+		maxz:-0.1
+	}
+	verify_box2
+	{
+		minx:0.982
+		maxx:1.192
+		miny:0.15
+		maxy:4.91
+		minz:-1.53
+		maxz:-0.1
+	}
+}
+
+
+
+

laser/lds_lidar.cpp

@@ -179,7 +179,8 @@ void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket *data,
 
     LdsLidar *lidar_this = static_cast<LdsLidar *>(client_data);
     //printf("receive handle %d   ptr %p\n", handle,client_data);
-    lidar_this->LidarDataCallback(handle,data,data_num);
+    if(lidar_this!= nullptr)
+        lidar_this->LidarDataCallback(handle,data,data_num);
     /*LivoxEthPacket *eth_packet = data;
 
     if (!data || !data_num || (handle >= kMaxLidarCount))
@@ -258,22 +259,30 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
         printf("sn:%s has not added map size:%d\n",info->broadcast_code,g_snPtr.size());
         return;
     }*/
+
+    if(!g_snPtr.find(info->broadcast_code))
+    {
+        return;
+    }
+
     livox_status result = kStatusFailure;
     uint8_t handle = 0;
     result = AddLidarToConnect(info->broadcast_code, &handle);
     if (result == kStatusSuccess && handle < kMaxLidarCount)
     {
-        LdsLidar* lidar=g_snPtr[info->broadcast_code];
 
-        SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void*)lidar);
+            LdsLidar *lidar = g_snPtr[info->broadcast_code];
+
+            SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void *) lidar);
+
+            LidarDevice *p_lidar = &(lidars_[handle]);
+            p_lidar->handle = handle;
+            p_lidar->connect_state = kConnectStateOff;
+            p_lidar->config.enable_fan = true;
+            p_lidar->config.return_mode = kStrongestReturn;
+            p_lidar->config.coordinate = kCoordinateCartesian;
+            p_lidar->config.imu_rate = kImuFreq200Hz;
 
-        LidarDevice *p_lidar = &(lidars_[handle]);
-        p_lidar->handle = handle;
-        p_lidar->connect_state = kConnectStateOff;
-        p_lidar->config.enable_fan = true;
-        p_lidar->config.return_mode = kStrongestReturn;
-        p_lidar->config.coordinate = kCoordinateCartesian;
-        p_lidar->config.imu_rate = kImuFreq200Hz;
     }
     else
     {

main.cpp

@@ -130,7 +130,6 @@ int main(int argc,char* argv[])
         return -1;
     }
 
-
     safety_excutor* excutor_entrance=safety_excutor::CreateExcutor(setting_parameter.entrance_parameter());
     safety_excutor* excutor_export=safety_excutor::CreateExcutor(setting_parameter.export_parameter());
 
@@ -174,9 +173,15 @@ int main(int argc,char* argv[])
         memset(&measure_data_entrance,0,sizeof(measure_data_entrance));
         memset(&measure_data_export,0,sizeof(measure_data_export));
         if(excutor_entrance!= nullptr)
+        {
+            printf(" entrance <<<<<<  ");
             excutor_entrance->verify(measure_data_entrance);
+        }
         if(excutor_export!= nullptr)
+        {
+            printf(" export   >>>>>>  ");
             excutor_export->verify(measure_data_export);
+        }
 
 
         struct PLCData plc_data;

plc/snap7_communication_base.cpp

@@ -286,7 +286,7 @@ Error_manager Snap7_communication_base::write_data_buf(Snap7_buf& snap7_buf)
 		std::unique_lock<std::mutex> lck(m_communication_lock);
 		unsigned short a=0;
 		memcpy(&a,snap7_buf.mp_buf_reverse,2);
-		printf("id %d  start %d  size :%d   value :%d\n",snap7_buf.m_id, snap7_buf.m_start_index, snap7_buf.m_size,a);
+		//printf("id %d  start %d  size :%d   value :%d\n",snap7_buf.m_id, snap7_buf.m_start_index, snap7_buf.m_size,a);
 		int result = m_snap7_client.AsDBWrite(snap7_buf.m_id, snap7_buf.m_start_index, snap7_buf.m_size,
 											  snap7_buf.mp_buf_reverse);
 

safety_excutor.cpp

@@ -25,12 +25,13 @@ bool safety_excutor::init(const shutter::shutter_param& parameter)
     if(parameter.lidars_size()==0)
         return false;
     mp_lidars=new livox::LivoxMid70[parameter.lidars_size()];
+    const float DEGREE=M_PI/180.0;
     for(int i=0;i<parameter.lidars_size();++i)
     {
         shutter::lidar_parameter ld_p=parameter.lidars(i);
         mp_lidars[i].AddBroadcastCode(parameter.lidars(i).sncode());
         mp_lidars[i].SetFPS(parameter.lidars(i).fps());
-        mp_lidars[i].SetTransformParam(Eigen::Vector3f(ld_p.r(),ld_p.p(),ld_p.y()),
+        mp_lidars[i].SetTransformParam(Eigen::Vector3f(ld_p.r()*DEGREE,ld_p.p()*DEGREE,ld_p.y()*DEGREE),
                 Eigen::Vector3f(ld_p.tx(),ld_p.ty(),ld_p.tz()));
     }
     m_parameter=parameter;
@@ -90,8 +91,6 @@ bool safety_excutor::verify(tagMeasureData& data)
         (*cloud)+=(*cloud_t);
     }
 
-
-
     shutter::box_param box=m_parameter.box();
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inner=PassThroughCloud(cloud,box);
 
@@ -149,35 +148,37 @@ bool safety_excutor::verify(tagMeasureData& data)
 #if VIEW
 
 
-    viewer.removeAllPointClouds();
-    viewer.removeShape("cube1");
-    viewer.removeShape("cube2");
+    viewer.removePointCloud(m_parameter.ShortDebugString()+"cloud");
+    viewer.removeShape(m_parameter.ShortDebugString()+"cube1");
+    viewer.removeShape(m_parameter.ShortDebugString()+"cube2");
     //绘制矩形
     Eigen::AngleAxisf rotation = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());
 
     viewer.addCube(Eigen::Vector3f((box1.minx()+box1.maxx())/2.0,(box1.miny()+box1.maxy())/2.0,(box1.minz()+box1.maxz())/2.0),
-            Eigen::Quaternionf(rotation), box1.maxx()-box1.minx(), box1.maxy()-box1.miny(),box1.maxz()-box1.minz(),"cube1");
+            Eigen::Quaternionf(rotation), box1.maxx()-box1.minx(), box1.maxy()-box1.miny(),box1.maxz()-box1.minz(),
+                   m_parameter.ShortDebugString()+"cube1");
     viewer.addCube(Eigen::Vector3f((box2.minx()+box2.maxx())/2.0,(box2.miny()+box2.maxy())/2.0,(box2.minz()+box2.maxz())/2.0),
-                   Eigen::Quaternionf(rotation), box2.maxx()-box2.minx(), box2.maxy()-box2.miny(),box2.maxz()-box2.minz(),"cube2");
-    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0.5, 0, "cube1");
-    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "cube1");
+                   Eigen::Quaternionf(rotation), box2.maxx()-box2.minx(), box2.maxy()-box2.miny(),box2.maxz()-box2.minz(),
+                   m_parameter.ShortDebugString()+"cube2");
+    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0.5, 0, m_parameter.ShortDebugString()+"cube1");
+    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, m_parameter.ShortDebugString()+"cube1");
 
-    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0.5, 1, "cube2");
-    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "cube2");
+    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0.5, 1, m_parameter.ShortDebugString()+"cube2");
+    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, m_parameter.ShortDebugString()+"cube2");
 
 
 
     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud,
             255*int(!data.wheel_path_safety),255*int(data.wheel_path_safety),255*int(!data.parkspcae_safety));
-    viewer.addPointCloud(cloud, single_color2, "cloud");
-    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
+    viewer.addPointCloud(cloud, single_color2, m_parameter.ShortDebugString()+"cloud");
+    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, m_parameter.ShortDebugString()+"cloud");
 
     viewer.spinOnce();
 #endif
     printf(" shutter  recieve points %d l_space:%.3f  r_space:%.3f  wheel_safety: %d   parkspace_safety: %d\n",
             cloud->size(),data.left_space,data.right_space,data.wheel_path_safety,data.parkspcae_safety);
     //保存一帧
-    static bool saved=false;
+    /*static bool saved=false;
     if(saved==false)
     {
         FILE* p=fopen("./cloud.txt","w");
@@ -187,7 +188,7 @@ bool safety_excutor::verify(tagMeasureData& data)
         }
         fclose(p);
         saved=true;
-    }
+    }*/
 
     return data.wheel_path_safety;