Browse Source

puai minor changes. bug fixed for the condition of wj_lidar writing to plc

yct 5 years ago
parent
commit
57c1330ac5

+ 1 - 0
.gitignore

@@ -44,3 +44,4 @@ _ReSharper*/
 packages/
 build/
 >>>>>>> origin/yct
+.idea/**

+ 3 - 3
CMakeLists.txt

@@ -44,9 +44,9 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/wj_lidar WJLIDAR_SRC )
 
 
 # plc test module
-add_executable(plc_test  ./test/plc_test.cpp ${PLC_SRC} ${TASK_MANAGER_SRC} ${TOOL_SRC} ${ERROR_CODE_SRC})
-target_link_libraries(plc_test ${OpenCV_LIBS}
-        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ipopt modbus libnanomsg.so libnnxx.a libglog.a libgflags.a)
+#add_executable(plc_test  ./test/plc_test.cpp ${PLC_SRC} ${TASK_MANAGER_SRC} ${TOOL_SRC} ${ERROR_CODE_SRC})
+#target_link_libraries(plc_test ${OpenCV_LIBS}
+#        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ipopt modbus libnanomsg.so libnnxx.a libglog.a libgflags.a)
 add_executable(fence_debug  test/plc_s7.cpp wj_lidar/wj_lidar_msg.pb.cc)
 target_link_libraries(fence_debug ${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES} /usr/local/lib/libglog.a
         /usr/local/lib/libgflags.a nnxx nanomsg)

+ 10 - 9
laser/Laser.cpp

@@ -243,7 +243,7 @@ Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
 		char pts_file[255] = { 0 };
 		sprintf(pts_file, "%s/pts%d.txt", save_path.c_str(), m_laser_id+1);
 		m_points_log_tool.open(pts_file);
-		std::cout << "huli m_points_log_tool path "<< pts_file << std::endl;
+		//std::cout << "huli m_points_log_tool path "<< pts_file << std::endl;
 
 		char bin_file[255] = { 0 };
 		sprintf(bin_file, "%s/laser%d.data", save_path.c_str(), m_laser_id+1);
@@ -284,6 +284,7 @@ void Laser_base::thread_receive()
 	//接受雷达消息,每次循环只接受一个 Binary_buf
 	while (m_condition_receive.is_alive())
 	{
+
 		m_condition_receive.wait();
 		if ( m_condition_receive.is_alive() )
 		{
@@ -335,20 +336,20 @@ void Laser_base::thread_transform()
 			int huli_test = m_queue_laser_data.size();
 			//thread_receive存入前要手动分配内存,thread_transform取出后要记得释放。
 			bool t_pop_flag = m_queue_laser_data.try_pop(tp_binaty_buf);
-			if ( t_pop_flag )
+			/*if ( t_pop_flag )
 			{
 			    std::cout << "huli m_queue_laser_data.size() last = " << huli_test << std::endl;
 				std::cout << "huli m_queue_laser_data.size() now = " << m_queue_laser_data.size() << std::endl;
 
-			}
+			}*/
 
 			//第2步,处理数据,缓存转化缓存为三维点。
 			//如果获取到了 Binary_buf,或者上次有未完成的 m_last_data,那么就执行数据转化。
 			//注注注注注意了,扫描停止后,m_last_data一定要保证清空,不能有垃圾数据残留,这会阻塞线程的暂停
 			if (t_pop_flag || m_last_data.get_length() > 0)
 			{
-				std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
-				std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
+				//std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
+				//std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
 				//缓存类型
 				Buf_type t_buf_type = BUF_UNKNOW;
 				//雷达扫描结果,三维点云(雷达自身坐标系)
@@ -396,7 +397,7 @@ void Laser_base::thread_transform()
 							char buf[64] = {0};
 							sprintf(buf, "%f %f %f\n", t_point.x, t_point.y, t_point.z);
 							m_points_log_tool.write(buf, strlen(buf));
-//							std::cout << "huli m_points_log_tool.write" << std::endl;
+							//std::cout << "huli m_points_log_tool.write" << std::endl;
 						}
 
 						//此时 t_point 为雷达采集数据的结果,转换后的三维点云。
@@ -453,7 +454,7 @@ void Laser_base::thread_transform()
 Error_manager Laser_base::publish_laser_to_message()
 {
 	return Error_code::SUCCESS;
-	/*
+/*
 	globalmsg::msg msg;
 	msg.set_msg_type(globalmsg::eLaser);
 
@@ -467,8 +468,8 @@ Error_manager Laser_base::publish_laser_to_message()
 	msg.mutable_laser_msg()->set_laser_status(status);
 	msg.mutable_laser_msg()->set_queue_data_count(m_queue_laser_data.size());
 	msg.mutable_laser_msg()->set_cloud_count(m_points_count);
-	MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
-	 */
+	MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());*/
+
 }
 //线程执行函数,公开发布雷达的相关信息,用作上位机的监视。
 void Laser_base::thread_publish(Laser_base *p_laser)

+ 12 - 11
laser/LivoxLaser.cpp

@@ -87,9 +87,10 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 	{
 		m_frame_maxnum=mp_laser_task->get_task_frame_maxnum();
 
+
 		//设置保存文件的路径
 		std::string save_path = mp_laser_task->get_task_save_path();
-		t_error = set_open_save_path(save_path,m_laser_param.is_save_txt());
+		t_error = set_open_save_path(save_path);
 		if ( t_error != Error_code::SUCCESS )
 		{
 			//文件保存文件的路径的设置 允许失败。继续后面的动作
@@ -171,7 +172,7 @@ bool CLivoxLaser::receive_buf_to_queue(Binary_buf& binary_buf)
 	{
 		binary_buf = *tp_livox_buf;
 		delete tp_livox_buf;
-		std::cout << "huli m_queue_livox_data.try_pop" << std::endl;
+		//std::cout << "huli m_queue_livox_data.try_pop" << std::endl;
 		return true;
 	}
 	return false;
@@ -382,15 +383,15 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 		{
 			if (livox->IsScanComplete())
 			{
-				std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
+				//std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
 				livox->stop_scan();
 				return;
 			}
-			else
+			/*else
 			{
-				std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
+				//std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
 
-			}
+			}*/
 //			std::cout << "huli LidarDataCallback " << std::endl;
 
 			if (g_count[handle] >= livox->m_frame_maxnum)
@@ -400,7 +401,7 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 
 			Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
 			livox->m_queue_livox_data.push(data_bin);
-			std::cout << "huli m_queue_livox_data.push " << std::endl;
+			//std::cout << "huli m_queue_livox_data.push " << std::endl;
 
 			g_count[handle]++;
 
@@ -415,9 +416,9 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 			usleep(10*1000);
 		}*/
 	}
-	else
+	/*else
 	{
-		std::cout << "huli 2z error " << "data = "<< data  << std::endl;
-		std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
-	}
+		//std::cout << "huli 2z error " << "data = "<< data  << std::endl;
+		//std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
+	}*/
 }

+ 12 - 0
laser/laser_task_command.cpp

@@ -171,6 +171,18 @@ void Laser_task::set_task_error_manager(Error_manager & error_manager)
     m_task_error_manager = error_manager;
 }
 
+//获取采集的点云保存路径
+std::string Laser_task::get_save_path()
+{
+    return m_save_path;
+}
+//设置采集的点云保存路径
+void Laser_task::set_save_path(std::string save_path)
+{
+    m_save_path=save_path;
+}
+
+
 
 
 

+ 4 - 0
laser/laser_task_command.h

@@ -83,6 +83,10 @@ public:
     //设置 错误码
     void set_task_error_manager(Error_manager & error_manager);
 
+    //获取采集的点云保存路径
+    std::string get_save_path();
+    //设置采集的点云保存路径
+    void set_save_path(std::string save_path);
 
 protected:
     //点云的采集帧数最大值,任务输入

+ 2 - 1
terminor/terminal_command_executor.cpp

@@ -349,7 +349,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
                 //
                 laser_task->task_init(TASK_CREATED,scan_cloud,&cloud_lock);
                 laser_task->set_task_frame_maxnum(frame_count);
-                laser_task->set_task_save_path(project_path);
+                laser_task->set_save_path(project_path);
                 laser_task_vector.push_back(laser_task);
                 //发送任务单给雷达
                 code=tp_lasers[i]->execute_task(laser_task);
@@ -365,6 +365,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
         return Error_manager(TERMINOR_NOT_CONTAINS_LASER,NORMAL,"terminal not contains laser");
     }
     m_terminor_statu=TERMINOR_SCANNING;
+
     //等待雷达完成任务单
     while(1)
     {

+ 4 - 4
wj_lidar/region_detect.cpp

@@ -134,12 +134,12 @@ Error_manager Region_detector::isRect(std::vector<cv::Point2f> &points, bool pri
 
         float width = std::min(l1, l2);
         float length = std::max(l1, l2);
-        // 车宽应位于[1.4, 2.0],车长应位于[2.2, 3.0]
-        if (width < 1.400 || width > 2.000 || length > 3.000 || length < 2.200)
+        // 车宽应位于[1.35, 2.0],车长应位于[2.2, 3.0]
+        if (width < 1.350 || width > 2.000 || length > 3.000 || length < 2.200)
         {
             if (print)
             {
-                LOG(WARNING) << "\t width<1400 || width >2100 || length >3300 ||length < 2100 "
+                LOG(WARNING) << "\t width<1350 || width >2100 || length >3300 ||length < 2100 "
                              << "  length:" << length << "  width:" << width;
             }
             return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR);
@@ -478,4 +478,4 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud
     }
 
     return Error_manager(Error_code::SUCCESS);
-}
+}

+ 3 - 2
wj_lidar/region_worker.cpp

@@ -175,9 +175,10 @@ void Region_worker::detect_loop(Region_worker *worker)
             int duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - worker->m_update_plc_time).count();
             Plc_data *p = Plc_data::get_instance();
             // 写入间隔必须超过500ms,当前状态不同于上次写入状态,且该状态已连续读到三次
-            if (p!=0 && duration > 500 && worker->m_last_sent_code != worker->m_last_read_code && worker->m_read_code_count > 3)
+            if (p!=0 && duration > 500 && ((worker->m_last_sent_code != worker->m_last_read_code) || worker->m_last_border_status != border_status) && worker->m_read_code_count > 3)
             {
                 worker->m_last_sent_code = worker->m_last_read_code;
+		worker->m_last_border_status = border_status;
                 p->update_data(code, border_status, worker->m_detector->get_region_id());
                 worker->m_update_plc_time = std::chrono::steady_clock::now();
             }
@@ -216,4 +217,4 @@ cv::RotatedRect Region_worker::create_rotate_rect(float length,float width,float
         point_vec.push_back(cv::Point2f(x,y));
     }
     return cv::minAreaRect(point_vec);
-}
+}

+ 1 - 0
wj_lidar/region_worker.h

@@ -52,6 +52,7 @@ private:
     std::chrono::steady_clock::time_point m_update_plc_time; // 更新plc状态时刻
     int m_last_sent_code;                              // 上次写入plc的状态值
     int m_last_read_code;                              // 上次检查获得的状态值
+    int m_last_border_status;				// 上次超界提示
     std::atomic<int> m_read_code_count;                      // 检查后重复获取相同状态值次数
 
     StdCondition m_cond_exit;     // 系统退出标志