增加测量结果对比并写入文件

build/sys.prototxt

@@ -13,9 +13,9 @@ lidar
 	port:2367
 	calib
 	{
-		y:-3.0
-		cx:3.36
-		cy:-0.02
+		y:3.0
+		cx:-3.36
+		cy:-0.15
 	}
 	
 }

src/communication/communication_message.h

@@ -59,6 +59,8 @@ public:
 
         eEntrance_manual_operation_msg=0xb0,            //针对出入口状态操作的手动消息
         eProcess_manual_operation_msg=0xb1,
+
+        eGround_measure_statu_msg=0xc0
     };
 
 //通讯单元

src/ground_region.cpp

@@ -285,12 +285,6 @@ Error_manager Ground_region::init(std::string configure_file)
 }
 Error_manager Ground_region::init(ground_region::Configure configure)
 {
-    //绑定通讯ip
-    char connect_str[255]={0};
-    sprintf(connect_str,"tcp://%s:%d",configure.communication_cfg().ip().c_str(),
-            configure.communication_cfg().port());
-    Publisher::get_instance_pointer()->communication_bind(connect_str);
-    Publisher::get_instance_pointer()->communication_run();
 
     //创建雷达
     m_lidars.resize(configure.lidar_size());
@@ -306,10 +300,17 @@ Error_manager Ground_region::init(ground_region::Configure configure)
         }
     }
 
-
-
     m_configure=configure;
     m_running_thread=new std::thread(std::bind(thread_measure_func,this));
+
+    //绑定通讯ip
+    char connect_str[255]={0};
+    sprintf(connect_str,"tcp://%s:%d",configure.communication_cfg().ip().c_str(),
+            configure.communication_cfg().port());
+    Publisher::get_instance_pointer()->communication_bind(connect_str);
+    Publisher::get_instance_pointer()->communication_run();
+    Publisher::get_instance_pointer()->set_cakkback(recieve_result_callback,this);
+
     return SUCCESS;
 }
 
@@ -424,6 +425,9 @@ Error_manager Ground_region::prehandle_cloud(std::vector<pcl::PointCloud<pcl::Po
         }
         filtered_cloud_vector.push_back(cloud_filtered);
     }
+
+
+
     //根据点云判断雷达状态，若所有雷达滤波后都有点， 则正常,lidar_statu_bit 各个位代表雷达是否有点， 有点为0，无点为1
     int lidar_statu_bit = 0;
     for (int i = 0; i < filtered_cloud_vector.size(); ++i)
@@ -439,6 +443,7 @@ Error_manager Ground_region::prehandle_cloud(std::vector<pcl::PointCloud<pcl::Po
         return SUCCESS;
     }
 
+
     //判断是否有雷达故障
     for (int i = 0; i < filtered_cloud_vector.size(); ++i)
     {
@@ -519,16 +524,15 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
 #include "message_base.pb.h"
 void Ground_region::thread_measure_func(Ground_region* p)
 {
-    message::Ground_measure_statu_msg msg;
     message::Base_info base_info;
     base_info.set_msg_type(message::eGround_measure_statu_msg);
     base_info.set_sender(message::eEmpty);
     base_info.set_receiver(message::eEmpty);
-    msg.mutable_base_info()->CopyFrom(base_info);
-    msg.set_ground_statu(message::Nothing);
+    p->m_latest_result.mutable_base_info()->CopyFrom(base_info);
+    p->m_latest_result.set_ground_statu(message::Nothing);
     for(int i=0;i<p->m_lidars.size();++i)
     {
-        msg.add_laser_statu_vector(message::LASER_DISCONNECT);
+        p->m_latest_result.add_laser_statu_vector(message::LASER_DISCONNECT);
     }
     const float fps=10.;
     //保持 10 fps
@@ -538,74 +542,199 @@ void Ground_region::thread_measure_func(Ground_region* p)
         Error_manager code=p->sync_capture(clouds,0.5);
         if(code!=SUCCESS)
         {
+            //采集失败， 不发送消息
             LOG(WARNING)<<code.get_error_description();
             continue;
         }
 
 
-        static int save=0;
-        save++;
-        if(save==10)
         {
-            for(int i=0;i<clouds.size();++i)
+            static int save = 0;
+
+            save++;
+            if (save == 10)
             {
-                std::cout<<"-------------------save  size:"<<clouds[i]->size()<<std::endl;
-                char file[255]={0};
-                sprintf(file,"./%d.txt",i);
-                save_cloud_txt(clouds[i],file);
+                for (int i = 0; i < clouds.size(); ++i)
+                {
+                    std::cout << "-------------------save  size:" << clouds[i]->size() << std::endl;
+                    char file[255] = {0};
+                    sprintf(file, "./%d.txt", i);
+                    save_cloud_txt(clouds[i], file);
+                }
+                printf("  saved \n");
             }
-            printf("  saved \n");
         }
 
 
         Tick tick;
-        code=p->prehandle_cloud(clouds,msg);
-        if(code!=SUCCESS)
+        code=p->prehandle_cloud(clouds,p->m_latest_result);
+        if(code ==POINT_EMPTY)
         {
-            LOG(WARNING)<<code.get_error_description()<< msg.laser_statu_vector_size()<<", "<<clouds.size();
-            continue;
+            //区域无点
+            p->m_latest_result.set_ground_statu(message::Nothing);
         }
-        detect_wheel_ceres::Detect_result result;
-        pcl::PointCloud<pcl::PointXYZ>::Ptr correct_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-        for(int i=0;i<clouds.size();++i)
+        else if(code == SUCCESS )
         {
-            *correct_cloud+=(*clouds[i]);
-        }
-        code=p->detect(correct_cloud,p->m_configure.box(),result);
+            //点云正常， 所有雷达均有点
+            detect_wheel_ceres::Detect_result result;
+            pcl::PointCloud<pcl::PointXYZ>::Ptr correct_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+            for(int i=0;i<clouds.size();++i)
+            {
+                *correct_cloud+=(*clouds[i]);
+            }
+            code=p->detect(correct_cloud,p->m_configure.box(),result);
+
+            if(tick.tic()>1./fps)
+            {
+                LOG(WARNING)<<" detect fps < capture fps";
+            }
+            if(code==SUCCESS)
+            {
+                //  成功，结果赋值到消息 ，更新消息状态到 测量正确, to be 需更新上下超界状态
+                p->m_latest_result.set_ground_statu(message::Car_correct);
+                message::Locate_information locate_information;
+                locate_information.set_locate_x(result.cx);
+                locate_information.set_locate_y(result.cy);
+                locate_information.set_locate_angle(result.theta);
+                locate_information.set_locate_wheel_base(result.wheel_base);
+                locate_information.set_locate_length(result.wheel_base);
+                locate_information.set_locate_wheel_width(result.width);
+                locate_information.set_locate_width(result.body_width);
+                locate_information.set_locate_front_theta(result.front_theta);
+                locate_information.set_locate_correct(true);
+
+                p->m_latest_result.mutable_locate_information_realtime()->CopyFrom(locate_information);
+
+                LOG(INFO)<<locate_information.DebugString()<<std::endl;
+            }
+            else{
+                //识别失败， 噪声
+                p->m_latest_result.set_ground_statu(message::Noise);
+            }
 
-        if(tick.tic()>1./fps)
-        {
-            LOG(WARNING)<<" detect fps < capture fps";
         }
-        if(code!=SUCCESS)
+        else
         {
-            LOG_EVERY_N(INFO,20)<<code.get_error_description();
-            continue;
+            p->m_latest_result.set_ground_statu(message::Noise);
         }
 
-        //  to be 发布结果
-        msg.set_ground_statu(message::Car_correct);
-        message::Locate_information locate_information;
-        locate_information.set_locate_x(result.cx);
-        locate_information.set_locate_y(result.cy);
-        locate_information.set_locate_angle(result.theta);
-        locate_information.set_locate_wheel_base(result.wheel_base);
-        locate_information.set_locate_length(result.wheel_base);
-        locate_information.set_locate_wheel_width(result.width);
-        locate_information.set_locate_width(result.body_width);
-        locate_information.set_locate_front_theta(result.front_theta);
-        locate_information.set_locate_correct(true);
-
-        msg.mutable_locate_information_realtime()->CopyFrom(locate_information);
+        //更新结果的最新时间（用于结果对比）
+        p->m_latest_tick=Tick();
 
         Communication_message c_msg;
-        c_msg.reset(msg.base_info(),msg.SerializeAsString());
+        c_msg.reset(p->m_latest_result.base_info(),p->m_latest_result.SerializeAsString());
         Publisher::get_instance_pointer()->publish_msg(&c_msg);
 
-        LOG(INFO)<<locate_information.DebugString()<<std::endl;
-
     }
 }
 
 
+#include "pathcreator.h"
+#include <fstream>
+void Ground_region::recieve_result_callback(void* p,Communication_message* p_msg)
+{
+    if(p== nullptr || p_msg== nullptr)
+        return ;
+
+    Ground_region* ground=(Ground_region*)p;
+    if(p_msg->get_message_type()!=Communication_message::eGround_measure_statu_msg)
+        return ;
+
+
+    message::Ground_measure_statu_msg msg;
+    if(msg.ParseFromString(p_msg->get_message_buf())==false)
+        return ;
+
+    if(msg.ground_statu()==message::Car_correct)
+    {
+        static int compare_count=0;
+        compare_count++;
+
+        char line[255]={0};
+        message::Locate_information wj_info=msg.locate_information_realtime();
+        sprintf(line,"%d %.3f %.3f %.3f %.3f %.3f %.3f - ",compare_count,
+                wj_info.locate_x(),wj_info.locate_y(),wj_info.locate_angle(),wj_info.locate_front_theta(),
+                wj_info.locate_width(),wj_info.locate_wheel_base());
+        if(ground->m_latest_tick.tic()>0.15)
+        {
+            sprintf(line+strlen(line),"unmeasured\n");
+        }
+        else
+        {
+            if(ground->m_latest_result.ground_statu()==message::Car_correct)
+            {
+                message::Locate_information vlp_info = ground->m_latest_result.locate_information_realtime();
+                double distance=sqrt(pow(vlp_info.locate_x()-wj_info.locate_x(),2)+
+                        pow(vlp_info.locate_y()-wj_info.locate_y(),2));
+
+                double v_xy=sin(wj_info.locate_angle()*M_PI/180.0)*distance;
+                double off_theta=fabs(vlp_info.locate_angle()-wj_info.locate_angle())*M_PI/180.0;
+                double offset=v_xy+ tan(off_theta)*2.5;
+                if(offset>0.02)
+                {
+                    sprintf(line+strlen(line),
+                            "%.3f %.3f %.3f %.3f %.3f %.3f\n",
+                            vlp_info.locate_x(),
+                            vlp_info.locate_y(),
+                            vlp_info.locate_angle(),
+                            vlp_info.locate_front_theta(),
+                            vlp_info.locate_width(),
+                            vlp_info.locate_wheel_base());
+                }
+            }
+            else
+            {
+                sprintf(line+strlen(line),"result is not Car_correct\n");
+            }
+        }
+
 
+        time_t tt = time(0);//时间cuo
+        struct tm* t = localtime(&tt);
+
+        char strYear[255]={0};
+        char strMonth[255]={0};
+        char strDay[255]={0};
+
+        sprintf(strYear,"%04d", t->tm_year+1900);
+        sprintf(strMonth,"%02d", t->tm_mon+1);
+        sprintf(strDay,"%02d", t->tm_mday);
+
+        char buf[255]={0};
+        getcwd(buf,255);
+        char strdir[255]={0};
+        sprintf(strdir,"%s/compare/%s/%s", buf,strYear,strMonth);
+        PathCreator creator;
+        creator.Mkdir(strdir);
+
+        std::string file;
+        file+=strdir;
+        file+=+"/";
+        file+=strDay;
+        file+=".txt";
+
+        static std::string latest_file=file;
+        static std::ofstream ofs(file.c_str(),std::ios_base::app);
+
+        if(file!=latest_file)
+        {
+            ofs.close();
+            ofs.open(file,std::ios_base::app);
+            latest_file=file;
+        }
+
+        if (!ofs)
+        {
+            return ;
+        }
+        // copy each character to the stream
+        ofs<<line;
+        ofs.flush();
+
+
+    }
+
+
+
+
+}

src/ground_region.h

@@ -23,6 +23,7 @@ using google::protobuf::Message;
 
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
+#include "publisher.h"
 #include "ground_measure_msg.pb.h"
 
 class Ground_region
@@ -48,6 +49,7 @@ class Ground_region
                                detect_wheel_ceres::Detect_result& result);
 
     static void thread_measure_func(Ground_region* p);
+    static void recieve_result_callback(void* p,Communication_message* p_msg);
 
 
 
@@ -63,6 +65,11 @@ class Ground_region
 
     detect_wheel_ceres                      m_detector;
 
+
+ protected:
+    message::Ground_measure_statu_msg       m_latest_result;
+    Tick                                    m_latest_tick;
+
 };
 
 

src/publisher.cpp

@@ -6,9 +6,32 @@
 Publisher::~Publisher(){}
 Publisher::Publisher()
 {
+    m_callback= nullptr;
+    m_p= nullptr;
 }
 
+
+
 Error_manager Publisher::publish_msg(Communication_message* p_msg)
 {
     return Communication_socket_base::encapsulate_msg(p_msg);
 }
+
+
+void Publisher::set_cakkback(callback func,void* p)
+{
+    m_callback=func;
+    m_p=p;
+}
+
+//处理消息, 需要子类重载
+Error_manager Publisher::execute_msg(Communication_message* p_msg)
+{
+    if(p_msg->get_message_type()==Communication_message::eGround_measure_statu_msg)
+    {
+        if(m_callback&& m_p)
+        {
+            m_callback(m_p,p_msg);
+        }
+    }
+}

src/publisher.h

@@ -8,6 +8,9 @@
 #include "singleton.h"
 #include "communication_socket_base.h"
 
+
+typedef void (*callback)(void*,Communication_message* p_msg);
+
 class Publisher : public Communication_socket_base,public Singleton<Publisher>
 {
     friend class Singleton<Publisher>;
@@ -15,11 +18,19 @@ class Publisher : public Communication_socket_base,public Singleton<Publisher>
     Publisher(const Publisher& other)=delete;
     Publisher& operator=(const Publisher& other)=delete;
 
+    void set_cakkback(callback func,void* p);
+
     Error_manager publish_msg(Communication_message* msg);
     ~Publisher();
 
  protected:
     Publisher();
+    //处理消息, 需要子类重载
+    virtual Error_manager execute_msg(Communication_message* p_msg);
+
+
+    callback                m_callback;
+    void*                   m_p;
 };