|
@@ -19,53 +19,110 @@ Laser_base::Laser_base(int laser_id, Laser_proto::laser_parameter laser_param)
|
|
|
}
|
|
|
Laser_base::~Laser_base()
|
|
|
{
|
|
|
- m_queue_laser_data.termination_queue();
|
|
|
+
|
|
|
+ disconnect_laser();
|
|
|
close_save_path();
|
|
|
- m_condition_receive.kill_all();
|
|
|
- m_condition_transform.kill_all();
|
|
|
- m_condition_publish.kill_all();
|
|
|
}
|
|
|
|
|
|
//雷达链接设备,为3个线程添加线程执行函数。
|
|
|
Error_manager Laser_base::connect_laser()
|
|
|
{
|
|
|
- m_bThreadRcvRun.notify_all(true);
|
|
|
- if(m_ThreadRcv==0)
|
|
|
- m_ThreadRcv = new std::thread(&Laser_base::thread_recv, this);
|
|
|
- m_ThreadRcv->detach();
|
|
|
-
|
|
|
- m_bThreadProRun.notify_all(true);
|
|
|
- if(m_ThreadPro==0)
|
|
|
- m_ThreadPro = new std::thread(&Laser_base::thread_toXYZ, this);
|
|
|
- m_ThreadPro->detach();
|
|
|
+ //检查雷达状态
|
|
|
+ if ( m_laser_statu != LASER_DISCONNECT )
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::LASER_STATUS_ERROR, Error_level::MINOR_ERROR,
|
|
|
+ " Laser_base::connect_laser m_laser_statu != LASER_DISCONNECT ");
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //这里不建议用detach,而是应该在disconnect里面使用join
|
|
|
+ //创建接受缓存的线程,不允许重复创建
|
|
|
+ if (mp_thread_receive != NULL)
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
|
|
|
+ " mp_thread_receive is alive ");
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //接受缓存的线程,线程存活,暂停运行
|
|
|
+ m_condition_receive.reset(false, false, false);
|
|
|
+ mp_thread_receive = new std::thread(&Laser_base::thread_receive, this);
|
|
|
+ }
|
|
|
+ //转化数据的线程,不允许重复创建
|
|
|
+ if (mp_thread_transform != NULL)
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
|
|
|
+ " mp_thread_transform is alive ");
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //转化数据的线程,线程存活,暂停运行
|
|
|
+ m_condition_transform.reset(false, false, false);
|
|
|
+ mp_thread_transform = new std::thread(&Laser_base::thread_transform, this);
|
|
|
+ }
|
|
|
+ //发布信息的线程,不允许重复创建
|
|
|
+ if (mp_thread_publish != NULL)
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
|
|
|
+ " mp_thread_publish is alive ");
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //发布信息的线程,线程存活,循环运行,
|
|
|
+ //注:mp_thread_publish 需要线程持续不断的往上位机发送信息,不受任务的影响。
|
|
|
+ m_condition_publish.reset(false, true, false);
|
|
|
+ mp_thread_publish = new std::thread(&Laser_base::thread_publish, this);
|
|
|
+ }
|
|
|
|
|
|
- if(m_ThreadPub==0)
|
|
|
- m_ThreadPub = new std::thread(threadPublish, this);
|
|
|
- m_ThreadPub->detach();
|
|
|
+ m_laser_statu = LASER_READY;
|
|
|
|
|
|
+ }
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
//雷达断开链接,释放3个线程
|
|
|
Error_manager Laser_base::disconnect_laser()
|
|
|
{
|
|
|
- if (m_ThreadRcv)
|
|
|
+ //终止队列,防止 wait_and_pop 阻塞线程。
|
|
|
+ m_queue_laser_data.termination_queue();
|
|
|
+ //杀死3个线程,强制退出
|
|
|
+ if (mp_thread_receive)
|
|
|
{
|
|
|
- delete m_ThreadRcv;
|
|
|
- m_ThreadRcv = 0;
|
|
|
+ m_condition_receive.kill_all();
|
|
|
}
|
|
|
-
|
|
|
- if (m_ThreadPub)
|
|
|
+ if (mp_thread_transform)
|
|
|
+ {
|
|
|
+ m_condition_transform.kill_all();
|
|
|
+ }
|
|
|
+ if (mp_thread_publish)
|
|
|
+ {
|
|
|
+ m_condition_publish.kill_all();
|
|
|
+ }
|
|
|
+ //回收3个线程的资源
|
|
|
+ if (mp_thread_receive)
|
|
|
+ {
|
|
|
+ mp_thread_receive->join();
|
|
|
+ delete mp_thread_receive;
|
|
|
+ mp_thread_receive = NULL;
|
|
|
+ }
|
|
|
+ if (mp_thread_transform)
|
|
|
+ {
|
|
|
+ mp_thread_transform->join();
|
|
|
+ delete mp_thread_transform;
|
|
|
+ mp_thread_transform = 0;
|
|
|
+ }
|
|
|
+ if (mp_thread_publish)
|
|
|
{
|
|
|
- delete m_ThreadPub;
|
|
|
- m_ThreadPub = 0;
|
|
|
+ mp_thread_publish->join();
|
|
|
+ delete mp_thread_publish;
|
|
|
+ mp_thread_publish = NULL;
|
|
|
}
|
|
|
+ //清空队列
|
|
|
+ m_queue_laser_data.clear_and_delete();
|
|
|
|
|
|
- if (m_ThreadPro)
|
|
|
+ if ( m_laser_statu != LASER_FAULT )
|
|
|
{
|
|
|
- delete m_ThreadPro;
|
|
|
- m_ThreadPro = NULL;
|
|
|
+ m_laser_statu = LASER_DISCONNECT;
|
|
|
}
|
|
|
- m_laser_statu = LASER_DISCONNECT;
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
|
|
@@ -90,92 +147,83 @@ Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
|
|
|
//检查雷达状态,是否正常运行
|
|
|
Error_manager Laser_base::check_laser()
|
|
|
{
|
|
|
- return Error_code::SUCCESS;
|
|
|
+ if ( is_ready() )
|
|
|
+ {
|
|
|
+ return Error_code::SUCCESS;
|
|
|
+ }
|
|
|
+ return Error_manager(Error_code::LASER_CHECK_FAILED, Error_level::MINOR_ERROR,
|
|
|
+ " check_laser failed ");
|
|
|
}
|
|
|
//雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
|
|
|
Error_manager Laser_base::start_scan()
|
|
|
{
|
|
|
- m_laser_scan_flag=true;
|
|
|
- m_laser_statu = LASER_BUSY;
|
|
|
- m_bStart_capture.notify_all(true);
|
|
|
- m_points_count=0;
|
|
|
- return Error_code::SUCCESS;
|
|
|
+ LOG(INFO) << " Laser_base::start_scan "<< this;
|
|
|
+ if ( is_ready() )
|
|
|
+ {
|
|
|
+ //启动雷达扫描
|
|
|
+ m_laser_scan_flag=true; //启动雷达扫描
|
|
|
+ m_laser_statu = LASER_BUSY; //雷达状态 繁忙
|
|
|
+ m_condition_receive.notify_all(true); //唤醒接受线程
|
|
|
+ m_condition_transform.notify_all(true); //唤醒转化线程
|
|
|
+ //重置数据
|
|
|
+ m_points_count=0;
|
|
|
+ m_queue_laser_data.clear_and_delete();
|
|
|
+ m_last_data.clear();
|
|
|
+ return Error_code::SUCCESS;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::LASER_START_FAILED, Error_level::MINOR_ERROR,
|
|
|
+ "Laser_base::start_scan() is not ready ");
|
|
|
+ }
|
|
|
}
|
|
|
//雷达的停止接口函数, 让雷达停止扫描,一般需要子类重载,不同的雷达结束方式不同。
|
|
|
Error_manager Laser_base::stop_scan()
|
|
|
{
|
|
|
+ LOG(INFO) << " Laser_base::stop_scan "<< this;
|
|
|
+ //stop_scan 只是将 m_laser_scan_flag 改为 stop。
|
|
|
+ //然后多线程内部判断 m_laser_scan_flag,然后自动停止线程
|
|
|
m_laser_scan_flag=false;
|
|
|
- m_bStart_capture.notify_all(false);
|
|
|
- m_binary_log_tool.close();
|
|
|
- m_points_log_tool.close();
|
|
|
- m_laser_statu=LASER_READY;
|
|
|
-
|
|
|
- //在停止雷达时,将雷达任务状态改为 TASK_OVER 已结束
|
|
|
- //强制改为TASK_OVER,不管它当前在做什么。
|
|
|
- if(mp_laser_task !=NULL)
|
|
|
- {
|
|
|
- mp_laser_task->set_task_statu(TASK_OVER);
|
|
|
- }
|
|
|
-
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
|
|
|
//结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
|
|
|
Error_manager Laser_base::end_task()
|
|
|
{
|
|
|
+ LOG(INFO) << " Laser_base::end_task "<< this;
|
|
|
+ m_laser_scan_flag=false;
|
|
|
+ m_condition_receive.notify_all(false);
|
|
|
+ m_condition_transform.notify_all(false);
|
|
|
+ m_points_count=0;
|
|
|
+ m_queue_laser_data.clear_and_delete();
|
|
|
+ m_last_data.clear();
|
|
|
+
|
|
|
+ close_save_path();
|
|
|
+
|
|
|
+ //在结束任务单时,将雷达任务状态改为 TASK_OVER 已结束
|
|
|
+ if(mp_laser_task !=NULL)
|
|
|
+ {
|
|
|
+ //判断任务单的错误等级,
|
|
|
+ if ( mp_laser_task->get_task_error_manager().get_error_level() > Error_level::MINOR_ERROR)
|
|
|
+ {
|
|
|
+ if ( m_laser_statu == LASER_BUSY )
|
|
|
+ {
|
|
|
+ //故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR,则恢复待机,可以接受下一次的任务单
|
|
|
+ m_laser_statu = LASER_READY;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //故障等级为 MAJOR_ERROR CRITICAL_ERROR,则雷达故障,不可以再接受任务单。
|
|
|
+ m_laser_statu = LASER_FAULT;
|
|
|
+ }
|
|
|
+ //强制改为TASK_OVER,不管它当前在做什么。
|
|
|
+ mp_laser_task->set_task_statu(TASK_OVER);
|
|
|
+ }
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
-/*
|
|
|
-//对外的接口函数,负责接受并处理任务单,
|
|
|
-//input:laser_task 雷达任务单,必须是子类,并且任务正确。(传引用)
|
|
|
-Error_manager Laser_base::porform_task(Laser_task* p_laser_task)
|
|
|
-{
|
|
|
- Error_manager t_error_manager;
|
|
|
- //检查指针
|
|
|
- if(p_laser_task == NULL)
|
|
|
- {
|
|
|
- return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
|
|
|
- "Laser_base::porform_task failed, POINTER_IS_NULL");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //接受任务,并将任务的状态改为TASK_SIGNED已签收
|
|
|
- mp_laser_task = p_laser_task;
|
|
|
- mp_laser_task->set_task_statu(TASK_SIGNED);
|
|
|
- }
|
|
|
-
|
|
|
- //检查消息内容是否正确,
|
|
|
- //检查点云指针
|
|
|
- if(p_laser_task->get_task_point3d_cloud_vector() == NULL)
|
|
|
- {
|
|
|
- t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
|
|
|
- Error_level::MINOR_ERROR,
|
|
|
- "Laser_base::porform_task failed, POINTER_IS_NULL");
|
|
|
- }
|
|
|
- //检查任务类型
|
|
|
- else if(p_laser_task->get_task_type() != LASER_TASK)
|
|
|
- {
|
|
|
- t_error_manager.error_manager_reset(Error_code::LASER_TASK_PARAMETER_ERROR,
|
|
|
- Error_level::MINOR_ERROR,
|
|
|
- "Laser_base::porform_task failed, task_type is error");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- //将任务的状态改为 TASK_WORKING 处理中
|
|
|
- mp_laser_task->set_task_statu(TASK_WORKING);
|
|
|
-
|
|
|
- //启动雷达扫描
|
|
|
- Start();
|
|
|
- }
|
|
|
-
|
|
|
- //返回错误码
|
|
|
- if(t_error_manager != Error_code::SUCCESS)
|
|
|
- {
|
|
|
- mp_laser_task->set_task_error_manager(t_error_manager);
|
|
|
- }
|
|
|
- return t_error_manager;
|
|
|
-}
|
|
|
-*/
|
|
|
+
|
|
|
|
|
|
|
|
|
//设置保存文件的路径,并打开文件,
|
|
@@ -195,25 +243,30 @@ 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;
|
|
|
|
|
|
char bin_file[255] = { 0 };
|
|
|
sprintf(bin_file, "%s/laser%d.data", save_path.c_str(), m_laser_id+1);
|
|
|
m_binary_log_tool.open(bin_file,true);
|
|
|
+ std::cout << "huli m_binary_log_tool path "<< bin_file << std::endl;
|
|
|
+
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
}
|
|
|
//关闭保存文件,推出前一定要执行
|
|
|
Error_manager Laser_base::close_save_path()
|
|
|
{
|
|
|
+ m_save_flag = false;
|
|
|
m_points_log_tool.close();
|
|
|
m_binary_log_tool.close();
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
|
|
|
//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
|
|
|
+//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
|
|
|
bool Laser_base::is_ready()
|
|
|
{
|
|
|
- return ( m_laser_statu == LASER_READY );
|
|
|
+ return ( get_laser_statu() == LASER_READY );
|
|
|
}
|
|
|
//获取雷达状态
|
|
|
Laser_statu Laser_base::get_laser_statu()
|
|
@@ -226,135 +279,183 @@ int Laser_base::get_laser_id()
|
|
|
return m_laser_id;
|
|
|
}
|
|
|
|
|
|
-
|
|
|
-void Laser_base::thread_recv()
|
|
|
+//线程执行函数,将二进制消息存入队列缓存,
|
|
|
+void Laser_base::thread_receive()
|
|
|
{
|
|
|
- while (m_bThreadRcvRun.wait_for_millisecond(1))
|
|
|
+ LOG(INFO) << " thread_receive start "<< this;
|
|
|
+ //接受雷达消息,每次循环只接受一个 Binary_buf
|
|
|
+ while (m_condition_receive.is_alive())
|
|
|
{
|
|
|
- if (m_bStart_capture.wait_for_millisecond(1) == false)
|
|
|
- continue;
|
|
|
- Binary_buf* data=new Binary_buf();
|
|
|
- if (this->RecvData(*data))
|
|
|
+ m_condition_receive.wait();
|
|
|
+ if ( m_condition_receive.is_alive() )
|
|
|
{
|
|
|
- m_queue_laser_data.push(data);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- delete data;
|
|
|
- data=0;
|
|
|
- }
|
|
|
+ //tp_binaty_buf的内存 需要手动分配,然后存入链表。
|
|
|
+ //m_queue_laser_data的内存由 Laser_base 基类管理。
|
|
|
+ Binary_buf* tp_binaty_buf = new Binary_buf();
|
|
|
+ //获取雷达的通信消息缓存
|
|
|
+ if (this->receive_buf_to_queue(*tp_binaty_buf))
|
|
|
+ {
|
|
|
+ //将缓存 存入队列。
|
|
|
+ //thread_receive存入前要手动分配内存,thread_transform取出后要记得释放。
|
|
|
+ m_queue_laser_data.push(tp_binaty_buf);
|
|
|
+ std::cout << "huli thread_receive " << m_queue_laser_data.size() << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ delete tp_binaty_buf;
|
|
|
+ tp_binaty_buf=NULL;
|
|
|
|
|
|
+ //接受线程无法接受到数据的同时,也收到了雷达停止扫描的信息,那么线程停止
|
|
|
+ //如果m_laser_scan_flag停止扫描,但是receive_buf_to_queue仍然能够接受到数据。那么线程继续运行。
|
|
|
+ //m_laser_scan_flag停止的瞬间,可能还有残留的数据,需要继续执行。
|
|
|
+ if ( !m_laser_scan_flag )
|
|
|
+ {
|
|
|
+ //停止线程,m_condition_receive.wait() 函数将会阻塞。
|
|
|
+ m_condition_receive.set_pass_ever(false);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
}
|
|
|
+ LOG(INFO) << " thread_receive end :"<<this;
|
|
|
+ return;
|
|
|
}
|
|
|
|
|
|
|
|
|
-
|
|
|
-void Laser_base::thread_toXYZ()
|
|
|
+//线程执行函数,转化并处理三维点云。
|
|
|
+void Laser_base::thread_transform()
|
|
|
{
|
|
|
- while (m_bThreadProRun.wait_for_millisecond(1))
|
|
|
+ LOG(INFO) << " thread_transform start "<< this;
|
|
|
+ //转化雷达数据,每次循环只转化一个 Binary_buf,然后得到 t_point3D_cloud.size() 个三维点,
|
|
|
+ while (m_condition_transform.is_alive())
|
|
|
{
|
|
|
- if (m_bStart_capture.wait_for_millisecond(1) == false)
|
|
|
+ m_condition_transform.wait();
|
|
|
+ if ( m_condition_transform.is_alive() )
|
|
|
{
|
|
|
- m_queue_laser_data.clear();
|
|
|
- continue;
|
|
|
- }
|
|
|
- Binary_buf* pData=NULL;
|
|
|
- bool bPop = m_queue_laser_data.try_pop(pData);
|
|
|
- Buf_type type = BUF_UNKNOW;
|
|
|
- if (bPop || m_last_data.get_length() != 0)
|
|
|
- {
|
|
|
- std::vector<CPoint3D> cloud;
|
|
|
- if (bPop)
|
|
|
+ //tp_binaty_buf的内存 从链表取出时,就自带内存了。需要手动释放。
|
|
|
+ //m_queue_laser_data的内存由 Laser_base 基类管理。
|
|
|
+ Binary_buf* tp_binaty_buf=NULL;
|
|
|
+ //第1步,从队列中取出缓存。
|
|
|
+ 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 (pData == NULL)
|
|
|
- continue;
|
|
|
- if(m_save_flag)
|
|
|
- {
|
|
|
- m_binary_log_tool.write(pData->get_buf(), pData->get_length());
|
|
|
- }
|
|
|
- type= this->Data2PointXYZ(pData, cloud);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- type = this->Data2PointXYZ(NULL, cloud);
|
|
|
- }
|
|
|
-
|
|
|
+ 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;
|
|
|
|
|
|
- if (type == BUF_UNKNOW)
|
|
|
- {
|
|
|
- delete pData;
|
|
|
- continue;
|
|
|
}
|
|
|
|
|
|
- if (type == BUF_DATA || type == BUF_START || type == BUF_STOP)
|
|
|
+ //第2步,处理数据,缓存转化缓存为三维点。
|
|
|
+ //如果获取到了 Binary_buf,或者上次有未完成的 m_last_data,那么就执行数据转化。
|
|
|
+ //注注注注注意了,扫描停止后,m_last_data一定要保证清空,不能有垃圾数据残留,这会阻塞线程的暂停
|
|
|
+ if (t_pop_flag || m_last_data.get_length() > 0)
|
|
|
{
|
|
|
- for (int i = 0; i < cloud.size(); ++i)
|
|
|
+ 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;
|
|
|
+ //雷达扫描结果,三维点云(雷达自身坐标系)
|
|
|
+ std::vector<CPoint3D> t_point3D_cloud;
|
|
|
+ //第2.1步,缓存转化为三维点。
|
|
|
+ if (t_pop_flag)
|
|
|
{
|
|
|
- CPoint3D point = transform_by_matrix(cloud[i]);
|
|
|
- if(m_save_flag) {
|
|
|
- char buf[64] = {0};
|
|
|
- sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
|
|
|
- m_points_log_tool.write(buf, strlen(buf));
|
|
|
- }
|
|
|
-
|
|
|
- //此时cloud为雷达采集数据的结果,转换后的三维点云。
|
|
|
- //将每个点存入任务单里面的点云容器。
|
|
|
-
|
|
|
- if(mp_laser_task!=NULL)
|
|
|
+ if (tp_binaty_buf == NULL)
|
|
|
{
|
|
|
- Error_manager code= mp_laser_task->task_push_point(pcl::PointXYZ(point.x,point.y,point.z));
|
|
|
+ //try_pop返回true,但是tp_binaty_buf没有数据,直接跳过。这个一般不可能出现。
|
|
|
+ continue;
|
|
|
}
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //保存雷达通信 二进制的源文件。
|
|
|
+ if(m_save_flag)
|
|
|
+ {
|
|
|
+ m_binary_log_tool.write(tp_binaty_buf->get_buf(), tp_binaty_buf->get_length());
|
|
|
+ }
|
|
|
+ //缓存转化为三维点。传入新的缓存,
|
|
|
+ t_buf_type= this->transform_buf_to_points(tp_binaty_buf, t_point3D_cloud);
|
|
|
+ }
|
|
|
}
|
|
|
- m_points_count+=cloud.size();
|
|
|
- if (type == BUF_STOP)
|
|
|
+ else if(m_last_data.get_length() > 0)
|
|
|
{
|
|
|
- if(m_save_flag) {
|
|
|
- m_points_log_tool.close();
|
|
|
- m_binary_log_tool.close();
|
|
|
- }
|
|
|
- m_laser_statu = LASER_READY;
|
|
|
+ //缓存转化为三维点。没有新消息,就传null,这将处理 m_last_data 上一次遗留的缓存。
|
|
|
+ t_buf_type = this->transform_buf_to_points(NULL, t_point3D_cloud);
|
|
|
+ }
|
|
|
|
|
|
- //在停止雷达时,将雷达任务状态改为 TASK_OVER 已结束
|
|
|
- if(mp_laser_task !=NULL)
|
|
|
+ //第2.2步,判断t_buf_type,处理雷达数据。并存入任务单
|
|
|
+ if (t_buf_type == BUF_UNKNOW || t_buf_type == BUF_READY)
|
|
|
+ {
|
|
|
+ //跳过这次循环。
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ else if (t_buf_type == BUF_DATA || t_buf_type == BUF_START || t_buf_type == BUF_STOP)
|
|
|
+ {
|
|
|
+ //循环处理雷达数据
|
|
|
+ for (int i = 0; i < t_point3D_cloud.size(); ++i)
|
|
|
{
|
|
|
- if(mp_laser_task->get_statu() == TASK_WORKING)
|
|
|
+ //三维点的坐标变换,从雷达自己的坐标系,转化到公共坐标系,(目前以plc为公共坐标系)
|
|
|
+ CPoint3D t_point = transform_by_matrix(t_point3D_cloud[i]);
|
|
|
+ //保存雷达扫描 三维点云的最终结果。
|
|
|
+ if(m_save_flag) {
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+ //此时 t_point 为雷达采集数据的结果,转换后的三维点云。
|
|
|
+ //将每个点存入任务单里面的点云容器。
|
|
|
+ if(mp_laser_task!=NULL)
|
|
|
{
|
|
|
- mp_laser_task->set_task_statu(TASK_OVER);
|
|
|
+ Error_manager t_error= mp_laser_task->task_push_point(pcl::PointXYZ(t_point.x,t_point.y,t_point.z));
|
|
|
+ if ( t_error != Error_code::SUCCESS )
|
|
|
+ {
|
|
|
+ mp_laser_task->get_task_error_manager().compare_and_cover_error(t_error);
|
|
|
+ stop_scan();
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
+ //统计扫描点个数。
|
|
|
+ m_points_count += t_point3D_cloud.size();
|
|
|
+ //思科雷达停止扫描。
|
|
|
+ if (t_buf_type == BUF_STOP)
|
|
|
+ {
|
|
|
+ //注:这个只是思科的雷达停止处,不同的雷达在不同的地方调用 stop_scan
|
|
|
+ stop_scan();
|
|
|
+ }
|
|
|
}
|
|
|
-
|
|
|
}
|
|
|
- else if (type == BUF_READY)
|
|
|
+ else
|
|
|
{
|
|
|
- if(m_save_flag) {
|
|
|
- m_points_log_tool.close();
|
|
|
- m_binary_log_tool.close();
|
|
|
+ //转化线程 不需要数据的同时,也收到了雷达停止扫描的信息并且接受线程已经停止,那么停止转化线程,
|
|
|
+ //注:必须先停止接受线程,再停止转化线程。转化线程停止之后即可结束任务。
|
|
|
+ //如果m_laser_scan_flag停止扫描,接受线程和转化线程还有未完成的数据。那么线程继续运行。
|
|
|
+ //m_laser_scan_flag停止的瞬间,可能还有残留的数据,需要继续执行。防止数据遗留。
|
|
|
+ if ( !m_laser_scan_flag && m_condition_receive.get_pass_ever() == false)
|
|
|
+ {
|
|
|
+ //停止线程,m_condition_transform.wait() 函数将会阻塞。
|
|
|
+ m_condition_transform.set_pass_ever(false);
|
|
|
+
|
|
|
+ //mp_thread_transform 停止时,雷达扫描任务彻底完成,此时结束任务
|
|
|
+ end_task();
|
|
|
}
|
|
|
- m_laser_statu = LASER_READY;
|
|
|
}
|
|
|
|
|
|
-
|
|
|
- delete pData;
|
|
|
+ //第3步,手动释放缓存
|
|
|
+ if ( tp_binaty_buf != NULL )
|
|
|
+ {
|
|
|
+ delete tp_binaty_buf;
|
|
|
+ }
|
|
|
}
|
|
|
-
|
|
|
}
|
|
|
+ LOG(INFO) << " thread_transform end "<< this;
|
|
|
+ return;
|
|
|
}
|
|
|
|
|
|
|
|
|
-#include "unistd.h"
|
|
|
-void Laser_base::threadPublish(Laser_base *laser)
|
|
|
-{
|
|
|
- if(laser==0) return ;
|
|
|
- while(laser->m_bThreadRcvRun.wait_for_millisecond(1))
|
|
|
- {
|
|
|
- laser->PublishMsg();
|
|
|
- usleep(1000*300);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void Laser_base::PublishMsg()
|
|
|
+//公开发布雷达信息的功能函数,
|
|
|
+Error_manager Laser_base::publish_laser_to_message()
|
|
|
{
|
|
|
+ return Error_code::SUCCESS;
|
|
|
/*
|
|
|
globalmsg::msg msg;
|
|
|
msg.set_msg_type(globalmsg::eLaser);
|
|
@@ -372,6 +473,30 @@ void Laser_base::PublishMsg()
|
|
|
MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
|
|
|
*/
|
|
|
}
|
|
|
+//线程执行函数,公开发布雷达的相关信息,用作上位机的监视。
|
|
|
+void Laser_base::thread_publish(Laser_base *p_laser)
|
|
|
+{
|
|
|
+ LOG(INFO) << " thread_publish start ";
|
|
|
+ if(p_laser==NULL)
|
|
|
+ {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ while (p_laser->m_condition_publish.is_alive())
|
|
|
+ {
|
|
|
+ p_laser->m_condition_publish.wait();
|
|
|
+ if ( p_laser->m_condition_publish.is_alive() )
|
|
|
+ {
|
|
|
+ p_laser->publish_laser_to_message();
|
|
|
+ //每隔300ms,发送一次雷达信息状态。
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ LOG(INFO) << " thread_publish end ";
|
|
|
+ return;
|
|
|
+}
|
|
|
+
|
|
|
|
|
|
|
|
|
//初始化变换矩阵,设置默认值
|