浏览代码

内存泄露未解决,现场代码

yct 5 年之前
父节点
当前提交
497abe1cf7

+ 1 - 1
laser/LivoxLaser.cpp

@@ -72,7 +72,7 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 
 	//检查消息内容是否正确,
 	//检查三维点云指针
-	if (mp_laser_task->get_task_point_cloud().get() == NULL)
+	if (mp_laser_task->get_task_point_cloud()== NULL)
 	{
 		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
 											Error_level::MINOR_ERROR,

+ 7 - 2
laser/LivoxMid100Laser.cpp

@@ -84,14 +84,19 @@ Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
 		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
 							 "laser task type error != LASER_TASK");
 	}
-
+    //检查当前状态
+    t_error=check_laser();
+	if(t_error!=SUCCESS)
+    {
+	    return t_error;
+    }
 	//接受任务,并将任务的状态改为TASK_SIGNED已签收
 	mp_laser_task = (Laser_task *) p_laser_task;
 	mp_laser_task->set_task_statu(TASK_SIGNED);
 
 	//检查消息内容是否正确,
 	//检查三维点云指针
-	if (mp_laser_task->get_task_point_cloud().get() == NULL)
+	if (mp_laser_task->get_task_point_cloud() == NULL)
 	{
 		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
 									 Error_level::MINOR_ERROR,

+ 7 - 7
laser/laser_task_command.cpp

@@ -32,10 +32,10 @@ Laser_task::~Laser_task()
 // input:p_task_cloud_lock 三维点云的数据保护锁
 //注:task_frame_maxnum默认为0,如果为0,则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
 Error_manager Laser_task::task_init(Task_statu task_statu,
-                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                                    pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                                     std::mutex* p_task_cloud_lock)
 {
-    if(p_task_point_cloud.get() == NULL)
+    if(p_task_point_cloud == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init p_task_point_cloud is null");
@@ -66,10 +66,10 @@ Error_manager Laser_task::task_init(Task_statu task_statu,
 Error_manager Laser_task::task_init(Task_statu task_statu,
                                     std::string & task_statu_information,
                                     unsigned int task_frame_maxnum,
-                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                                    pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                                     std::mutex* p_task_cloud_lock)
 {
-    if(p_task_point_cloud.get() == NULL)
+    if(p_task_point_cloud == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init p_task_point_cloud is null");
@@ -105,7 +105,7 @@ Error_manager Laser_task::task_push_point(pcl::PointXYZ point_xyz)
         return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
                              "push_point laser task input lock is null");
     }
-    if(mp_task_point_cloud.get()==NULL)
+    if(mp_task_point_cloud==NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_push_point p_task_point_cloud is null");
@@ -131,7 +131,7 @@ std::string Laser_task::get_task_save_path()
     return m_task_save_path;
 }
 //获取 三维点云容器的智能指针
-pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_task::get_task_point_cloud()
+pcl::PointCloud<pcl::PointXYZ>* Laser_task::get_task_point_cloud()
 {
     return mp_task_point_cloud;
 }
@@ -161,7 +161,7 @@ void Laser_task::set_task_save_path(std::string task_save_path)
     m_task_save_path=task_save_path;
 }
 //设置 三维点云容器的智能指针
-void Laser_task::set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
+void Laser_task::set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud)
 {
     mp_task_point_cloud = p_task_point_cloud;
 }

+ 6 - 6
laser/laser_task_command.h

@@ -31,7 +31,7 @@ public:
     //构造函数,构造函数锁定任务类型为LASER_TASK,后续不允许更改
     Laser_task();
     //析构函数
-    ~Laser_task();
+    virtual ~Laser_task();
 
     //初始化任务单,必须初始化之后才可以使用,(必选参数)
     // input:task_statu 任务状态
@@ -39,7 +39,7 @@ public:
     // input:p_task_cloud_lock 三维点云的数据保护锁
     //注:task_frame_maxnum默认为0,如果为0,则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
     Error_manager task_init(Task_statu task_statu,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                            pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                             std::mutex* p_task_cloud_lock);
 
     //初始化任务单,必须初始化之后才可以使用,(可选参数)
@@ -52,7 +52,7 @@ public:
     Error_manager task_init(Task_statu task_statu,
                             std::string & task_statu_information,
                             unsigned int task_frame_maxnum,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                            pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                             std::mutex* p_task_cloud_lock);
 
     //push点云,把 point_xyz 添加到 p_task_point_cloud 里面。
@@ -65,7 +65,7 @@ public:
     //获取采集的点云保存路径
     std::string get_task_save_path();
     //获取 三维点云容器的智能指针
-    pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
+    pcl::PointCloud<pcl::PointXYZ>* get_task_point_cloud();
 
 
 
@@ -79,7 +79,7 @@ public:
     //设置采集的点云保存路径
     void set_task_save_path(std::string task_save_path);
     //设置 三维点云容器的智能指针
-    void set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
+    void set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud);
     //设置 错误码
     void set_task_error_manager(Error_manager & error_manager);
 
@@ -101,7 +101,7 @@ protected:
     std::mutex*                     mp_task_cloud_lock;
     //采集结果,三维点云容器的智能指针,任务输出
     //这里只是指针,实际内存由应用层管理,初始化时必须保证内存有效。
-    pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+    pcl::PointCloud<pcl::PointXYZ>*        mp_task_point_cloud;
 
 
 };

+ 1 - 1
locate/locate_task.cpp

@@ -15,7 +15,7 @@ Locate_task::~Locate_task()
 
 Error_manager Locate_task::set_locate_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
 {
-    if(cloud.get()==0)
+    if(cloud==0)
     {
         return Error_manager(LOCATER_TASK_INPUT_CLOUD_UNINIT,NORMAL,"locate task set input cloud uninit");
     }

+ 1 - 1
locate/locate_task.h

@@ -31,7 +31,7 @@ class Locate_task: public Task_Base
 {
 public:
     Locate_task();
-    ~Locate_task();
+    virtual  ~Locate_task();
     //设置测量输入点云
     Error_manager set_locate_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
     //获取输入点云

+ 5 - 8
locate/locater.cpp

@@ -95,7 +95,7 @@ Error_manager Locater::execute_task(Task_Base* task,double time_out)
     ///第一步,获取task中input点云,文件保存路径
     pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_input=locate_task->get_locate_cloud();
     std::string save_path=locate_task->get_save_path();
-    if(t_cloud_input.get()==0)
+    if(t_cloud_input==0)
     {
         locate_task->update_statu(TASK_OVER,"Task set cloud is uninit");
         return Error_manager(LOCATER_INPUT_CLOUD_UNINIT,NORMAL,"Task set cloud is uninit");
@@ -189,7 +189,7 @@ Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
     maxp.x = minp.x + yolo_box.w;
     maxp.y = minp.y + yolo_box.h;
 
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input_sift(new pcl::PointCloud<pcl::PointXYZ>());
+    //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input_sift(new pcl::PointCloud<pcl::PointXYZ>());
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsample(new pcl::PointCloud<pcl::PointXYZ>());
     ///	///体素网格 下采样
     pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
@@ -226,13 +226,10 @@ Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
     {
         return Error_manager(LOCATER_SIFT_PREDICT_NO_CAR_POINT,NORMAL,"sift predict no car or wheel");
     }
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
     //第0类即是轮胎点云,第二类为车身点云
-    pcl::copyPointCloud(*segmentation_clouds[0], *t_cloud_wheel);
-    pcl::copyPointCloud(*segmentation_clouds[2], *t_cloud_car);
-    cloud_wheel=t_cloud_wheel;
-    cloud_car=t_cloud_car;
+    pcl::copyPointCloud(*segmentation_clouds[0], *cloud_wheel);
+    pcl::copyPointCloud(*segmentation_clouds[2], *cloud_car);
+
     return SUCCESS;
 }
 

+ 1 - 1
task/task_command_manager.h

@@ -29,7 +29,7 @@ enum Task_statu
 class Task_Base
 {
 public:
-    ~Task_Base();
+    virtual ~Task_Base();
     //初始化任务单,初始任务单类型为 UNKONW_TASK
     virtual Error_manager init();
     //更新任务单

+ 23 - 11
terminor/terminal_command_executor.cpp

@@ -7,6 +7,8 @@
 #include "terminor_msg.pb.h"
 #include "MeasureRequest.h"
 
+std::mutex Terminor_Command_Executor::ms_mutex_scan_measure;
+
 //////以下两个函数用于测试,读取指定点云
 bool string2point(std::string str,pcl::PointXYZ& point)
 {
@@ -295,6 +297,8 @@ void Terminor_Command_Executor::set_save_root_path(std::string root)
 
 Error_manager Terminor_Command_Executor::scanning_measuring()
 {
+    std::lock_guard<std::mutex> lock(ms_mutex_scan_measure);
+    LOG(INFO)<<">>>>>>>>>>>> Enter terminator ID:"<<m_terminor_parameter.id();
     //计时起点
     auto t_start_point=std::chrono::system_clock::now();
 
@@ -315,9 +319,9 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
     Error_manager code;
     //第一步,启动雷达扫描点云,切换当前状态为扫描中
     //根据配置筛选雷达
-    pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ> scan_cloud;
     std::mutex cloud_lock;
-    std::vector<Task_Base*> laser_task_vector;
+    std::vector<Laser_task*> laser_task_vector;
 
     std::vector<Laser_base*> tp_lasers;
     for(int i=0;i<m_terminor_parameter.laser_parameter_size();++i)
@@ -347,7 +351,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
                 //创建扫描任务,
                 Laser_task* laser_task=new Laser_task();
                 //
-                laser_task->task_init(TASK_CREATED,scan_cloud,&cloud_lock);
+                laser_task->task_init(TASK_CREATED,&scan_cloud,&cloud_lock);
                 laser_task->set_task_frame_maxnum(frame_count);
                 laser_task->set_save_path(project_path);
                 laser_task_vector.push_back(laser_task);
@@ -394,7 +398,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
         if(second>m_timeout_second)
         {
             //扫描超时,点云中没有点,则返回错误
-            if(scan_cloud->size()==0)
+            if(scan_cloud.size()==0)
             {
                 return Error_manager(TERMINOR_LASER_TIMEOUT,MAJOR_ERROR,"laser scanning timeout");
             }
@@ -404,7 +408,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
         usleep(1000*100);
     }
     //检查雷达任务完成情况,是否是正常完成
-    LOG(INFO)<<" laser scanning over cloud size:"<<scan_cloud->size();
+    LOG(INFO)<<" laser scanning over cloud size:"<<scan_cloud.size();
     //释放扫描任务单
     for(int i=0;i<laser_task_vector.size();++i)
     {
@@ -419,7 +423,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
     pcl::PassThrough<pcl::PointXYZ> passX;
     pcl::PassThrough<pcl::PointXYZ> passY;
     pcl::PassThrough<pcl::PointXYZ> passZ;
-    passX.setInputCloud(scan_cloud);
+    passX.setInputCloud(scan_cloud.makeShared());
     passX.setFilterFieldName("x");
     passX.setFilterLimits(m_terminor_parameter.area_3d().min_x(),m_terminor_parameter.area_3d().max_x());
     passX.filter(*locate_cloud);
@@ -460,6 +464,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
     code=wj_task->init();
     if(code!=SUCCESS)
     {
+        delete wj_task;
         return code;
     }
     wj_command t_wj_command;
@@ -470,6 +475,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
     code=mp_wj_lidar->execute_task(wj_task);
     if(code!=SUCCESS)
     {
+        delete wj_task;
         return code;
     }
     ///万集测量正确,对比两数据
@@ -477,6 +483,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
     code=wj_task->get_result(wj_result);
     if(code!=SUCCESS)
     {
+        delete wj_task;
         return code;
     }
     Locate_information wj_locate_information;
@@ -490,17 +497,21 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
     float offset_y=fabs(dj_locate_information.locate_y-wj_locate_information.locate_y);
     float offset_angle=fabs(dj_locate_information.locate_theta-wj_locate_information.locate_theta);
     float offset_width=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
-    float offset_wheel_base=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
-    if(offset_x>100 ||offset_y>100 ||offset_angle>2 || offset_wheel_base>200 ||offset_width>150)
+    float offset_wheel_base=fabs(dj_locate_information.locate_wheel_base-wj_locate_information.locate_wheel_base);
+    if(offset_x>100 ||offset_y>100 ||offset_angle>4 || offset_wheel_base>200 ||offset_width>200)
     {
-	LOG(WARNING)<<"chekc failed. (offset x>100, y>100, angle>2, wheel_base>200, width>100): "<<offset_x<<" "<<offset_y<<" "<<offset_angle<<" "<<offset_wheel_base<<" "<<offset_width;
+	LOG(WARNING)<<"chekc failed. (offset x>100, y>100, angle>4, wheel_base>200, width>200): "<<offset_x<<" "<<offset_y<<" "<<offset_angle<<" "<<offset_wheel_base<<" "<<offset_width;
         return Error_manager(TERMINOR_CHECK_RESULTS_ERROR,NORMAL,"check results failed ");
     }
     ///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以2750为基准作卡尔曼滤波
     ///车长以大疆雷达为准,车宽以万集雷达为准,高以大疆雷达为准
     m_measure_information.locate_x=wj_locate_information.locate_x;
     m_measure_information.locate_y=wj_locate_information.locate_y;
-    m_measure_information.locate_theta=0.5*(wj_locate_information.locate_theta+dj_locate_information.locate_theta);
+    if(offset_angle<2)
+        m_measure_information.locate_theta=0.5*wj_locate_information.locate_theta+0.5*dj_locate_information.locate_theta;
+    else
+        m_measure_information.locate_theta=0.1*wj_locate_information.locate_theta+0.9*dj_locate_information.locate_theta;
+
     float dj_distance=fabs(dj_locate_information.locate_wheel_base-2750);
     float wj_distance=fabs(wj_locate_information.locate_wheel_base-2750);
     float weight_dj=wj_distance/(dj_distance+wj_distance);
@@ -524,10 +535,11 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
                                         m_measure_information.locate_x, m_measure_information.locate_y);
         code = mp_verify_tool->verify(rotated_rect,m_measure_information.locate_hight, false);
         m_measure_information.locate_correct = (code == SUCCESS);
+        delete wj_task;
         return code;
 
     }
-
+    delete wj_task;
     return code;
 }
 

+ 4 - 0
terminor/terminal_command_executor.h

@@ -13,6 +13,7 @@
 #include "../wj_lidar/fence_controller.h"
 #include "Terminor_parameter.pb.h"
 #include <thread>
+#include <mutex>
 /*
  * 终端指令执行状态,枚举
  * 列举终端指令从进入流程到测量完成过程中的进度状态
@@ -111,6 +112,9 @@ protected:
     //检验结果工具
     Verify_result*              mp_verify_tool;
 
+    //
+    static std::mutex                  ms_mutex_scan_measure;
+
 };
 
 #endif //TERMINOR_H

+ 1 - 1
wj_lidar/plc_data.h

@@ -48,7 +48,7 @@ private:
     StdCondition m_cond_exit;                // 系统关闭标志
     std::thread *mp_update_thread;           // plc更新线程
     S7PLC m_plc;                             // S7协议句柄
-    const int m_update_interval_milli = 200; // plc更新频率
+    const int m_update_interval_milli = 100; // plc更新频率
 
     // 有参构造函数
     Plc_data(std::string ip);