|
@@ -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)
|