huli_test, 20200811

error_code/error_code.h

@@ -76,11 +76,14 @@ enum Error_code
 
 //    laser_base基类
 	LASER_BASE_ERROR_BASE			= 0x01010000,
-    LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达任务输入参数错误
-    LASER_CONNECT_FAILED,
-	LASER_START_FAILED,
-	LASER_CHECK_FAILED				,
-	LASER_STATUS_ERROR,								//雷达状态错误
+	LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达基类模块, 任务输入参数错误
+	LASER_CONNECT_FAILED,							//雷达基类模块, 连接失败
+	LASER_START_FAILED,								//雷达基类模块, 开始扫描失败
+	LASER_CHECK_FAILED,								//雷达基类模块, 检查失败
+	LASER_STATUS_BUSY,								//雷达基类模块, 状态正忙
+	LASER_STATUS_ERROR,								//雷达基类模块, 状态错误
+	LASER_TASK_OVER_TIME,							//雷达基类模块, 任务超时
+	LASER_QUEUE_ERROR,								//雷达基类模块, 数据缓存错误
 
 
     LASER_LIVOX_SKD_INIT_FAILED,

laser/Laser.cpp

@@ -224,7 +224,14 @@ Error_manager Laser_base::end_task()
 	return Error_code::SUCCESS;
 }
 
-
+//取消任务单，由发送方提前取消任务单
+Error_manager Laser_base::cancel_task()
+{
+	end_task();
+	//强制改为 TASK_DEAD，不管它当前在做什么。
+	mp_laser_task->set_task_statu(TASK_DEAD);
+	return Error_code::SUCCESS;
+}
 
 //设置保存文件的路径，并打开文件，
 Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)

laser/Laser.h

@@ -70,7 +70,8 @@ public:
 	virtual Error_manager stop_scan();
 	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
 	virtual Error_manager end_task();
-
+	//取消任务单，由发送方提前取消任务单
+	virtual Error_manager cancel_task();
 public:
 	//设置保存文件的路径，并打开文件，
 	Error_manager set_open_save_path(std::string save_path,bool is_save=true);

laser/LivoxLaser.cpp

@@ -1,6 +1,5 @@
 
 #include "LivoxLaser.h"
-#include <unistd.h>
 
 RegisterLaser(Livox)
 

laser/LivoxMid100Laser.cpp

@@ -166,7 +166,7 @@ Error_manager CLivoxMid100Laser::start_scan()
 //雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
 Error_manager CLivoxMid100Laser::stop_scan()
 {
-	LOG(INFO)<<" livoxmid100 stoped !!!"<<m_laser_param.sn()<<",tid:"<<std::this_thread::get_id();
+	LOG(INFO)<<" livoxmid100 stoped !!!"<<m_laser_param.sn();
 	return CLivoxLaser::stop_scan();
 }
 //结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task

laser/laser_task_command.cpp

@@ -97,6 +97,64 @@ Error_manager Laser_task::task_init(Task_statu task_statu,
     return Error_code::SUCCESS;
 }
 
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_frame_maxnum 点云的采集帧数最大值
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Laser_task::task_init(Task_statu task_statu,
+									std::string task_statu_information,
+									void* p_tast_receiver,
+									std::chrono::milliseconds task_over_time,
+									unsigned int task_frame_maxnum,
+									bool task_save_flag,
+									std::string task_save_path,
+									std::mutex* p_task_cloud_lock,
+									pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud
+)
+{
+	if(p_task_point_cloud == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Laser_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = task_statu;
+	m_task_statu_information = task_statu_information;
+	mp_tast_receiver = p_tast_receiver;
+	m_task_over_time = task_over_time;
+	m_task_error_manager.error_manager_clear_all();
+
+	if ( task_frame_maxnum == 0 )
+	{
+		m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
+	}
+	else
+	{
+		m_task_frame_maxnum = task_frame_maxnum;
+	}
+	//???????
+	m_save_path = task_save_path;
+	m_task_save_path = task_save_path;
+
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
+
+	return Error_code::SUCCESS;
+}
+
 //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
 Error_manager Laser_task::task_push_point(pcl::PointXYZ point_xyz)
 {

laser/laser_task_command.h

@@ -55,6 +55,28 @@ public:
                             pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
                             std::mutex* p_task_cloud_lock);
 
+	//初始化任务单，必须初始化之后才可以使用，（可选参数）
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	//    input:task_frame_maxnum 点云的采集帧数最大值
+	//    input:task_save_flag 是否保存
+	//    input:task_save_path 保存路径
+	//    input:p_task_cloud_lock 三维点云的数据保护锁
+	//    output:p_task_pmutexoint_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager task_init(Task_statu task_statu,
+							std::string task_statu_information,
+							void* p_tast_receiver,
+							std::chrono::milliseconds task_over_time,
+							unsigned int task_frame_maxnum,
+							bool task_save_flag,
+							std::string task_save_path,
+							std::mutex* p_task_cloud_lock,
+							pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud
+	);
+
     //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
     Error_manager task_push_point(pcl::PointXYZ point_xyz);
 

task/task_command_manager.cpp

@@ -3,48 +3,91 @@
 //
 
 #include "task_command_manager.h"
-#include "../error.h"
+#include "../error_code/error_code.h"
 
 Task_Base::Task_Base()
 {
-    m_task_type=UNKNOW_TASK;
-}
-Task_Base::~Task_Base()
-{
+	static unsigned int t_task_id = 0;
+	m_task_id = t_task_id;
+	t_task_id++;
 
+	m_task_type = UNKNOW_TASK;
+	m_task_statu = TASK_CREATED;
+	mp_tast_receiver = NULL;
+
+	m_task_start_time = std::chrono::system_clock::now();	//获取当前时间
+	m_task_over_time = std::chrono::milliseconds(TASK_OVER_TIME_DEFAULT); //默认10秒
 }
-//初始化任务单,初始任务单类型为 UNKONW_TASK
-Error_manager Task_Base::init()
+Task_Base::~Task_Base()
 {
-    m_task_type=UNKNOW_TASK;
-    return SUCCESS;
+	mp_tast_receiver = NULL;
 }
+
+
 //更新任务单
 //task_statu: 任务状态
 //statu_information:状态说明
 Error_manager Task_Base::update_statu(Task_statu task_statu,std::string statu_information)
 {
-    m_task_statu=task_statu;
-    m_task_statu_information=statu_information;
-    return SUCCESS;
+	m_task_statu=task_statu;
+	m_task_statu_information=statu_information;
+	return SUCCESS;
+}
+
+//判断是否超时。返回true表示任务超时，返回false表示任务没有超时
+bool Task_Base::is_over_time()
+{
+	return (std::chrono::system_clock::now() - m_task_start_time) > m_task_over_time;
 }
+
+//判断是否结束, TASK_OVER  TASK_ERROR TASK_DEAD 都算结束
+bool Task_Base::is_task_end()
+{
+	if(m_task_statu == TASK_OVER || m_task_statu == TASK_ERROR || m_task_statu == TASK_DEAD)
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+
+
+
+//获取 任务单id
+unsigned int  Task_Base::get_task_id()
+{
+	return m_task_id;
+}
+
 //获取任务类型
 Task_type Task_Base::get_task_type()
 {
-    return m_task_type;
+	return m_task_type;
 }
 //获取任务单状态
-Task_statu  Task_Base::get_statu()
+Task_statu  Task_Base::get_task_statu()
 {
-    return m_task_statu;
+	return m_task_statu;
+}
+//设置 任务单状态
+void  Task_Base::set_task_statu(Task_statu task_statu)
+{
+	m_task_statu = task_statu;
 }
 //获取状态说明
-std::string Task_Base::get_statu_information()
+std::string Task_Base::get_task_statu_information()
 {
-    return m_task_statu_information;
+	return m_task_statu_information;
 }
-
-//获取 错误码
+//设置 状态说明
+void Task_Base::set_task_statu_information(std::string task_statu_information)
+{
+	m_task_statu_information = task_statu_information;
+}
+//获取 错误码，返回引用。
 Error_manager& Task_Base::get_task_error_manager()
 {
 	return m_task_error_manager;
@@ -54,7 +97,48 @@ void Task_Base::set_task_error_manager(Error_manager & error_manager)
 {
 	m_task_error_manager = error_manager;
 }
+//比较覆盖错误码
+void Task_Base::compare_and_cover_task_error_manager(Error_manager & error_manager)
+{
+	m_task_error_manager.compare_and_cover_error(error_manager);
+}
+
+
+
+
+
+//获取任务接收方
+void * Task_Base::get_tast_receiver()
+{
+	return mp_tast_receiver;
+}
+//设置任务接收方
+void Task_Base::set_tast_receiver(void * p_tast_receiver)
+{
+	mp_tast_receiver = p_tast_receiver;
+}
+
 
+//获取 任务创建的时间点
+std::chrono::system_clock::time_point Task_Base::get_task_start_time()
+{
+	return m_task_start_time;
+}
+//设置 任务创建的时间点
+void Task_Base::set_task_start_time(std::chrono::system_clock::time_point task_start_time)
+{
+	m_task_start_time = task_start_time;
+}
+//获取 任务超时的时限
+std::chrono::milliseconds	Task_Base::get_task_over_time()
+{
+	return m_task_over_time;
+}
+//设置 任务超时的时限
+void	Task_Base::get_task_over_time(std::chrono::milliseconds task_over_time)
+{
+	m_task_over_time = task_over_time;
+}
 
 
 

task/task_command_manager.h

@@ -6,6 +6,10 @@
 #define TASK_COMAND_MANAGER_H
 #include <string>
 #include "../error_code/error_code.h"
+#include <chrono>
+
+//任务超时时间默认值10000ms，10秒
+#define TASK_OVER_TIME_DEFAULT				10000
 
 //任务类型
 enum Task_type
@@ -19,39 +23,91 @@ enum Task_type
 //任务状态，如果任务故障，任务状态改为TASK_OVER，然后在m_task_error_manager 补充错误码。
 enum Task_statu
 {
-    TASK_CREATED            =0,             //创建状态，默认值
-    TASK_SIGNED             =1,             //已签收
-    TASK_WORKING            =2,             //处理中
-    TASK_OVER               =3,             //已结束
+	TASK_CREATED            =0,             //创建状态，默认值
+	TASK_SIGNED             =1,             //已签收
+	TASK_WORKING            =2,             //处理中
+	TASK_OVER               =3,             //已结束
+	TASK_ERROR              =11,             //任务错误
+
+	//前面的状态由接收方选择, TASK_DEAD 则是发送方选择.
+	TASK_DEAD               =12,             //当任务超时,发送方让任务死亡，
+
 };
 
+
+
 //任务单基类
 class Task_Base
 {
+protected:
+	//不允许构造基类，只允许子类构造，（多态）
+	Task_Base();
 public:
-    virtual ~Task_Base();
-    //初始化任务单,初始任务单类型为 UNKONW_TASK
-    virtual Error_manager init();
-    //更新任务单
-    //task_statu: 任务状态
-    //statu_information:状态说明
-    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
-    //获取任务类型
-    Task_type   get_task_type();
-    //获取任务单状态
-    Task_statu  get_statu();
-    //获取状态说明
-    std::string get_statu_information();
-	//获取 错误码
+	~Task_Base();
+
+	//更新任务单
+	//task_statu: 任务状态
+	//statu_information:状态说明
+	Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
+
+	//判断是否超时。返回true表示任务超时，返回false表示任务没有超时
+	bool is_over_time();
+
+	//判断是否结束, TASK_OVER  TASK_ERROR TASK_DEAD 都算结束
+	bool is_task_end();
+
+public:
+
+	//获取 任务单id
+	unsigned int  get_task_id();
+	//设置 任务单id
+//	void  set_task_id(unsigned int task_id) = delete;
+
+	//获取 任务类型
+	Task_type   get_task_type();
+	//设置 任务类型
+//	void   set_task_type(Task_type task_type) = delete;
+	//获取 任务单状态
+	Task_statu  get_task_statu();
+	//设置 任务单状态
+	void  set_task_statu(Task_statu task_statu);
+	//获取 状态说明
+	std::string get_task_statu_information();
+	//设置 状态说明
+	void set_task_statu_information(std::string task_statu_information);
+	//获取 错误码，返回引用。
 	Error_manager& get_task_error_manager();
 	//设置 错误码
 	void set_task_error_manager(Error_manager & error_manager);
+	//比较覆盖错误码
+	void compare_and_cover_task_error_manager(Error_manager & error_manager);
+
+	//获取任务接收方
+	void * get_tast_receiver();
+	//设置任务接收方
+	void set_tast_receiver(void * p_tast_receiver);
+	//获取 任务创建的时间点
+	std::chrono::system_clock::time_point get_task_start_time();
+	//设置 任务创建的时间点
+	void set_task_start_time(std::chrono::system_clock::time_point task_start_time);
+	//获取 任务超时的时限
+	std::chrono::milliseconds	get_task_over_time();
+	//设置 任务超时的时限
+	void	get_task_over_time(std::chrono::milliseconds task_over_time);
+
+
 protected:
-    Task_Base();
-protected:
-    Task_type                   m_task_type;					//任务类型
-    Task_statu                  m_task_statu;					//任务状态
-    std::string					m_task_statu_information;		//任务状态说明
+	unsigned int				m_task_id;						//任务id, 每次新建任务, 自动+1, 用于多任务的管理
+	Task_type                   m_task_type;					//任务类型，不允许中途修改
+	Task_statu                  m_task_statu;					//任务状态
+	std::string					m_task_statu_information;		//任务状态说明
+	void*						mp_tast_receiver;				//任务接收方，Task_Base并不分配和释放内存。
+	//注：mp_tast_receiver是可选的，可以为NULL。如果为NULL，则需要task_command_manager去找到接收对象。
+
+	std::chrono::system_clock::time_point 	m_task_start_time;	//任务创建的时间点
+	std::chrono::milliseconds	m_task_over_time;				//任务超时的时限
+	//注：std::chrono::system_clock::now();	//获取当前时间
+
 	//错误码，任务故障信息，任务输出
 	Error_manager               m_task_error_manager;
 };

terminor/terminal_command_executor.cpp

@@ -298,6 +298,7 @@ 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);
     Error_manager code;
+	Error_manager t_result;
     LOG(INFO) << ">>>>>>>>>>>> Enter terminator ID:" << m_terminor_parameter.id();
     //计时起点
     auto t_start_point = std::chrono::system_clock::now();
@@ -363,45 +364,82 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     m_terminor_statu = TERMINOR_SCANNING;
 
     //等待雷达完成任务单
-    while (1) {
+    while (1)
+    {
         //判断是否强制退出
         if (mb_force_quit == true) {
             char description[255] = {0};
             sprintf(description, "ternimal is force quit terminal id : %d", m_terminor_parameter.id());
             return Error_manager(TERMINOR_FORCE_QUIT, NORMAL, description);
         }
-        //判断雷达任务单是否全部完成
-        bool tb_laser_complete = true;
-        for (int i = 0; i < laser_task_vector.size(); ++i) {
-            tb_laser_complete &= (TASK_OVER == laser_task_vector[i]->get_statu());
-        }
-        if (tb_laser_complete) {
-            break;
-        }
-        //计算扫描时间,若超时,并且没有点,则返回错误.
-        auto t_end_point = std::chrono::system_clock::now();
-        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
-        double second = double(duration.count()) *
-                        std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
-        if (second > m_timeout_second) {
-            //扫描超时,点云中没有点,则返回错误
-            if (scan_cloud.size() == 0) {
-                return Error_manager(TERMINOR_LASER_TIMEOUT, MAJOR_ERROR, "laser scanning timeout");
-            }
 
+		int i = 0;
+		for (; i < laser_task_vector.size(); ++i)
+		{
+			if (laser_task_vector[i]->is_task_end() == false)
+			{
+				if (laser_task_vector[i]->is_over_time())
+				{
+					//超时处理。取消任务。
+//                    tp_lasers[i]->cancel_task();
+                    tp_lasers[i]->stop_scan();
+					laser_task_vector[i]->set_task_statu(TASK_DEAD);
+					code.error_manager_reset(Error_code::LASER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
+												" LASER_TASK is_over_time ");
+					laser_task_vector[i]->set_task_error_manager(code);
+
+					continue;
+					//本次子任务超时死亡.直接进行下一个
+				}
+				else
+				{
+					//如果任务还在执行, 那就等待任务继续完成，等1ms
+					//std::this_thread::sleep_for(std::chrono::milliseconds(1));
+					std::this_thread::yield();
+					break;
+					//注意了:这里break之后,   i != laser_task_vector.size()
+					//这就表示仍然有子任务单没有结束.那就继续等待,,,直到所有任务完成或者超时.
+				}
+
+			}
+			//else 任务完成就判断下一个
+		}
+		//如果下发的任务全部结束，那么汇总所有的结果, 并销毁下发的任务单。
+		if(i == laser_task_vector.size() )
+		{
+			for ( int j = 0; j < laser_task_vector.size();  ++j)
+			{
+				if (laser_task_vector[i] != 0) {
+					//数据汇总，
+					//由于 pcl::PointCloud<pcl::PointXYZ>::Ptr mp_task_point_cloud; 是所有任务单共用的，所以不用处理了。自动就在一起。
+					//错误汇总
+					t_result.compare_and_cover_error(laser_task_vector[j]->get_task_error_manager());
+					//销毁下发的任务单。
+					delete laser_task_vector[i];
+					laser_task_vector[i] = NULL;
+				}
+			}
+			laser_task_vector.clear();
+		}
+		//else 直接通过, 然后重新进入  while (1)
 
-        }
         usleep(1000 * 100);
     }
+
+    //暂时不做错误处理,
+    // 如果有个别雷达扫描故障, 取消雷达子任务之后, 忽略错误
+    //用其他正常的雷达扫描点云, 继续后面的流程,
+    // 交给locate去定位识别, 如果识别不出来, 由locate来报错.
+	if ( t_result != SUCCESS )
+	{
+		LOG(INFO) << " laser_task error :::: "<< t_result.to_string()<< this;
+//		return t_result;
+	}
+
     //检查雷达任务完成情况,是否是正常完成
     LOG(INFO) << " laser scanning over cloud size:" << scan_cloud.size();
-    //释放扫描任务单
-    for (int i = 0; i < laser_task_vector.size(); ++i) {
-        if (laser_task_vector[i] != 0) {
-            delete laser_task_vector[i];
-            laser_task_vector[i] = NULL;
-        }
-    }
+
+
 
     //第二步,根据区域筛选点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -482,32 +520,20 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     float offset_width = fabs(dj_locate_information.locate_width - wj_locate_information.locate_width);
     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 > 550 || offset_width > 200) {
-        LOG(WARNING) << "chekc failed. (offset x>100, y>100, angle>4, wheel_base>550, width>200): " << offset_x
+    if (offset_x > 100 || offset_y > 150 || offset_angle > 5 || offset_wheel_base > 550 || offset_width > 250) {
+        LOG(WARNING) << "chekc failed. (offset x>100, y>150, angle>5, wheel_base>550, width>250): " << offset_x
                      << " " << offset_y << " " << offset_angle << " " << offset_wheel_base << " " << offset_width;
         return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
     }
-    ///根据两个结果选择最优结果,角度选择两值加权,轴距已以2750为基准作卡尔曼滤波
+    ///根据sigmod函数计算角度权重
+    double angle_weight=1.0-1.0/(1.0+exp(-offset_angle/1.8));
+    ///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以2750为基准作卡尔曼滤波
     ///车长以大疆雷达为准,车宽以万集雷达为准,高以大疆雷达为准
-    //m_measure_information.locate_x = wj_locate_information.locate_x;
-   // m_measure_information.locate_y = wj_locate_information.locate_y;
-    if (offset_angle < 2) {
-        float r1=0.5;
-        float r2=1.0-r1;
-        m_measure_information.locate_theta =
-                r1 * wj_locate_information.locate_theta + r2 * dj_locate_information.locate_theta;
-        m_measure_information.locate_x=r1*wj_locate_information.locate_x+r2*dj_locate_information.locate_x;
-        m_measure_information.locate_y=r1*wj_locate_information.locate_y+r2*dj_locate_information.locate_y;
-    }
+    m_measure_information.locate_x = wj_locate_information.locate_x;
+    m_measure_information.locate_y = wj_locate_information.locate_y;
 
-    else {
-        float r1=0.1;
-        float r2=1.0-r1;
-        m_measure_information.locate_theta =
-                r1 * wj_locate_information.locate_theta + r2 * dj_locate_information.locate_theta;
-        m_measure_information.locate_x=r1*wj_locate_information.locate_x+r2*dj_locate_information.locate_x;
-        m_measure_information.locate_y=r1*wj_locate_information.locate_y+r2*dj_locate_information.locate_y;
-    }
+    m_measure_information.locate_theta =angle_weight * wj_locate_information.locate_theta +
+            (1.0-angle_weight) * 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);