param tuning, publish seperate clouds, add min max xy. restruct

src/findwheel/CMakeLists.txt

@@ -153,16 +153,16 @@ link_directories(${PCL_LIBRARY_DIRS})
 
 add_executable(${PROJECT_NAME}_node 
     src/find_wheel_node.cpp
-    src/LogFiles.cpp
+    src/tools/LogFiles.cpp
     src/Lidar.cpp
     src/region_detect.cpp
     src/FenceController.cpp
-    src/EleFence.pb.cc
-    src/StdCondition.cpp
+    src/tools/EleFence.pb.cc
+    src/tools/StdCondition.cpp
     src/region_worker.cpp
-    src/globalmsg.pb.cc
-    src/s7_plc.cpp
-        src/PlcData.cpp)
+    src/tools/globalmsg.pb.cc
+    src/tools/s7_plc.cpp
+    src/PlcData.cpp)
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} snap7 libnnxx.a libnanomsg.so ${OpenCV_LIBS})
 
 ## Rename C++ executable without prefix

src/findwheel/scripts/EleFence.proto

@@ -24,6 +24,10 @@ message lidarParam
     optional string topic=1;
     optional Transform2d transform=2;
     optional float dist_limit=3 [default=8.0];
+    optional float minx=4 [default=-6];
+    optional float maxx=5 [default=-0.2];
+    optional float miny=6 [default=-3.5];
+    optional float maxy=7 [default=3.5];
 }
 
 message globalParam

src/findwheel/scripts/setting.prototxt

@@ -1,37 +1,101 @@
+
+lidar_params
+{
+    topic:"/scan3"
+	transform
+	{
+		m00:-0.0030665
+		m01:0.9999952
+		m02:5.27414
+		m10:0.9999952
+		m11:0.0030665
+		m12:-2.427125
+	}
+	minx:0.1
+    maxx:6
+    miny:-3.5
+    maxy:3.5
+	dist_limit:8
+}
+
 lidar_params
 {
-    topic:"/scan1"
+    topic:"/scan4"
 	transform
 	{
-		m00:0.0132359
-		m01:-0.9999126
-		m02:0.081
-		m10:-0.9999126
-		m11:-0.0132359
-		m12:5.057
+		m00:0.0259596
+		m01:-0.9996629
+		m02:8.75014
+		m10:-0.9996629
+		m11:-0.0259596
+		m12:3.450875
 	}
+	minx:0.1
+    maxx:6
+    miny:-3.5
+    maxy:3.5
 	dist_limit:8.0
 }
 
 lidar_params
 {
-    topic:"/scan2"
+    topic:"/scan5"
 	transform
 	{
-		m00:0.0134667
-		m01:0.9999093
-		m02:3.002
-		m10:0.9999093
-		m11:-0.0134667
-		m12:-0.803
+		m00:0.0225145
+		m01:0.9997465
+		m02:12.19914
+		m10:0.9997465
+		m11:-0.0225145
+		m12:-2.462125
 	}
+	minx:0.1
+    maxx:6
+    miny:-3.5
+    maxy:3.5
 	dist_limit:8.0
 }
 
+
+#1
+regions
+{
+	minx:5.4
+	maxx:8.7
+	miny:-2.43
+	maxy:3.43
+}
+
+#2
+regions
+{
+	minx:2.5
+	maxx:5.3
+	miny:-2.4
+	maxy:3.43
+}
+
+#3
+regions
+{
+	minx:5.4
+	maxx:8.7
+	miny:-2.40
+	maxy:3.43
+}
+#4
+regions
+{
+    minx:8.7
+    maxx:12
+    miny:-2.40
+    maxy:3.45
+}
+#5
 regions
 {
-	minx:0
-	maxx:3.3
-	miny:0
-	maxy:5.0
-}
+    minx:12.1
+    maxx:15.4
+    miny:-2.40
+    maxy:3.45
+}

src/findwheel/src/FenceController.cpp

@@ -1,7 +1,7 @@
 //
 // Created by zx on 2019/12/6.
 //
-#include "include/FenceController.h"
+#include "FenceController.h"
 
 bool FenceController::ReadProtoParam(std::string path)
 {
@@ -142,9 +142,10 @@ void FenceController::wheelMsgRecvThread(FenceController* fc)
 
 void FenceController::wheelMsgHandlingThread(FenceController* fc){
     if(fc==0) return;
-    int count = fc->m_retry_count >0 ? fc->m_retry_count:1;
-    while(!fc->m_cond_wheel_exit.WaitFor(1)){
 
+    while(!fc->m_cond_wheel_exit.WaitFor(1))
+    {
+        int count = fc->m_retry_count >0 ? fc->m_retry_count:1;
         MsgTask task;
         if(fc->m_msg_queue.try_pop(task))
         {
@@ -164,11 +165,12 @@ void FenceController::wheelMsgHandlingThread(FenceController* fc){
                 std::cout << "terminal id: " << terminal_id << std::endl;
                 while (count-- >= 0 && !result.correctness())
                 {
+                    std::cout<<" times :"<<count<<std::endl;
                     for (int i = 0; i < fc->p_region_workers.size(); ++i)
                     {
                         if (terminal_id >= 0 && fc->p_region_workers[i]->get_id() == terminal_id)
                         {
-                            // std::cout << "--------callback find terminal------" << terminal_id << std::endl;
+                             std::cout << "--------callback find terminal------" << terminal_id << std::endl;
                             double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
                             // std::cout << "--------callback get time------" << std::endl;
                             fc->m_cloud_mutex.lock();
@@ -182,7 +184,7 @@ void FenceController::wheelMsgHandlingThread(FenceController* fc){
                                 }
                             }
                             fc->m_cloud_mutex.unlock();
-                            // std::cout << "--------callback get cloud------" << std::endl;
+                             std::cout << "--------callback get cloud------" << std::endl;
                             if (total_cloud->size() > 0 &&
                                 fc->p_region_workers[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width))
                             {
@@ -192,7 +194,7 @@ void FenceController::wheelMsgHandlingThread(FenceController* fc){
                             {
                                 result.set_correctness(false);
                             }
-                            // std::cout << "--------callback get result------" << std::endl;
+                             std::cout << "--------callback get result------" << std::endl;
                             result.set_x(x * 1000.0);
                             result.set_y(y * 1000.0);
                             result.set_c(c);

src/findwheel/src/include/FenceController.h

@@ -20,14 +20,14 @@ using google::protobuf::io::CodedOutputStream;
 using google::protobuf::Message;
 
 #include <ros/ros.h>
-#include "EleFence.pb.h"
-#include "globalmsg.pb.h"
+#include "tools/EleFence.pb.h"
+#include "tools/globalmsg.pb.h"
 
-#include "LogFiles.h"
+#include "tools/LogFiles.h"
 #include "Lidar.h"
 #include "region_worker.h"
 #include "unistd.h"
-#include "s7_plc.h"
+#include "tools/s7_plc.h"
 #include "PlcData.h"
 #include "string.h"
 
@@ -41,7 +41,7 @@ using google::protobuf::Message;
 #include <cstring>
 #include <nnxx/message>
 
-#include "threadSafeQueue.h"
+#include "tools/threadSafeQueue.h"
 
 #ifndef FENCECONTROLLER_H
 #define FENCECONTROLLER_H

src/findwheel/src/Lidar.cpp

@@ -2,11 +2,11 @@
 // Created by zx on 2019/12/5.
 //
 
-#include "include/Lidar.h"
-#include "include/LogFiles.h"
+#include "Lidar.h"
+#include "tools/LogFiles.h"
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr  laserScan2Cloud(const sensor_msgs::LaserScan& msg,
-                                                     Eigen::MatrixXf transform2d, float dist_limit)
+                                                     Eigen::MatrixXf transform2d, float minx,float maxx,float miny,float maxy)
 {
     pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
@@ -15,7 +15,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr  laserScan2Cloud(const sensor_msgs::LaserSca
     {
         const float first_echo = msg.ranges[i];
         // within the border
-        if (msg.range_min <= first_echo && first_echo <= msg.range_max && dist_limit > first_echo)
+        if (msg.range_min <= first_echo && first_echo <= msg.range_max)
         {
             const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
             Eigen::MatrixXf  mat=rotation * (first_echo * Eigen::Vector3f::UnitX());
@@ -24,6 +24,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr  laserScan2Cloud(const sensor_msgs::LaserSca
             Eigen::MatrixXf  new_pos=tf_btol*e_transform;*/
             float x = mat(0, 0);
             float y = mat(1, 0);
+
+            if(x<minx||x>maxx||y<miny||y>maxy)
+            {
+                angle += msg.angle_increment;
+                continue;
+            }
             float newx = x * transform2d(0, 0) + y * transform2d(0, 1) + transform2d(0, 2);
             float newy = x * transform2d(1, 0) + y * transform2d(1, 1) + transform2d(1, 2);
             pcl::PointXYZ point(newx, newy, 0);
@@ -60,18 +66,31 @@ b_save_cloud(0)
 //    std::cout<<"topic: "<<lidar_param.topic()<<std::endl;
     m_scan_sub=m_handle.subscribe<sensor_msgs::LaserScan>(lidar_param.topic(), 1,
                                                           boost::bind(cloud_callback,_1,this));
+    std::string cloud_topic=lidar_param.topic()+"_cloud";
+    m_cloud_publisher = m_handle.advertise<sensor_msgs::PointCloud2>(cloud_topic, 1);
 }
 Lidar::~Lidar()
 {
 
 }
+void Lidar::publish_cloud()
+{
+    sensor_msgs::PointCloud2 cloudglobalMsg;
+    pcl::toROSMsg(*m_cloud, cloudglobalMsg);
+    cloudglobalMsg.header.stamp = ros::Time::now();
+    cloudglobalMsg.header.frame_id = "/map";
+    m_cloud_publisher.publish(cloudglobalMsg);
+}
 
 void Lidar::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& msg,Lidar* pLidar)
 {
 //    ROS_INFO("cloud callbacked");
     if(pLidar==0) return ;
     EleFence::Transform2d T=pLidar->m_lidar_param.transform();
-    float dist_limit = pLidar->m_lidar_param.dist_limit();
+    float minx=pLidar->m_lidar_param.minx();
+    float maxx=pLidar->m_lidar_param.maxx();
+    float miny=pLidar->m_lidar_param.miny();
+    float maxy=pLidar->m_lidar_param.maxy();
     Eigen::Matrix<float,2,3> transform;
     transform(0,0)=T.m00();
     transform(0,1)=T.m01();
@@ -79,19 +98,20 @@ void Lidar::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& msg,Lidar* pL
     transform(1,0)=T.m10();
     transform(1,1)=T.m11();
     transform(1,2)=T.m12();
-    pLidar->m_cloud=laserScan2Cloud(*msg,transform,dist_limit);
+    pLidar->m_cloud=laserScan2Cloud(*msg,transform,minx,maxx,miny,maxy);
     pLidar->m_cloud_time = ros::Time::now();
+    pLidar->publish_cloud();
 
     if(pLidar->b_save_cloud==false)
     {
         char buf[255] = {0};
         if (pLidar->m_lidar_param.topic().size() > 0 && pLidar->m_lidar_param.topic().at(0) == '/')
         {
-            sprintf(buf, "/home/youchen/data/wj_wheels/cloud_%s.txt", pLidar->m_lidar_param.topic().c_str() + 1);
+            sprintf(buf, "./cloud_%s.txt", pLidar->m_lidar_param.topic().c_str() + 1);
         }
         else
         {
-            sprintf(buf, "/home/youchen/data/wj_wheels/cloud_%s.txt", pLidar->m_lidar_param.topic().c_str());
+            sprintf(buf, "./cloud_%s.txt", pLidar->m_lidar_param.topic().c_str());
         }
         SaveTxtCloud(pLidar->m_cloud, buf);
         std::cout<<" save cloud   "<<buf<<std::endl;

src/findwheel/src/include/Lidar.h

@@ -5,7 +5,7 @@
 #ifndef LIDAR_H
 #define LIDAR_H
 #include <ros/ros.h>
-#include "EleFence.pb.h"
+#include "tools/EleFence.pb.h"
 #include <string>
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/point_types.h>
@@ -25,6 +25,7 @@ public:
     bool GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int timeout_milli=100);
 
 protected:
+    void publish_cloud();
     static void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& msg,Lidar* pLidar);
 
 protected:
@@ -33,6 +34,7 @@ protected:
     ros::Time   m_cloud_time;
 
     ros::Subscriber m_scan_sub;
+    ros::Publisher m_cloud_publisher;
     EleFence::lidarParam m_lidar_param;
 
     bool                b_save_cloud;

src/findwheel/src/PlcData.cpp

@@ -2,7 +2,7 @@
 // Created by zx on 2019/12/9.
 //
 
-#include "include/PlcData.h"
+#include "PlcData.h"
 #include <string.h>
 
 PlcData* PlcData::g_instance=0;

src/findwheel/src/include/PlcData.h

@@ -7,11 +7,11 @@
 #include <iostream>
 #include <string.h>
 #include <mutex>
-#include "StdCondition.h"
+#include "tools/StdCondition.h"
 #include <thread>
-#include "s7_plc.h"
+#include "tools/s7_plc.h"
 #include "unistd.h"
-#include "define.h"
+#include "tools/define.h"
 
 #define MAX_REGIONS 15
 #define ELE_FENCE_START_ADDR 0

src/findwheel/src/find_wheel_node.cpp

@@ -1,4 +1,4 @@
-#include "include/FenceController.h"
+#include "FenceController.h"
 
 
 ros::Publisher g_cluster_publisher[4];
@@ -14,8 +14,8 @@ int main(int argc, char** argv)
 //    }
 
     ros::init(argc, argv, "find_wheel");
-    // FenceController fc("/home/youchen/Documents/measure/MainStructure/elecfence_ws/src/findwheel/scripts/setting.prototxt");
-    FenceController fc("/home/zx/zzw/catkin_ws/src/findwheel/scripts/setting.prototxt");
+    FenceController fc("/home/youchen/Documents/measure/MainStructure/elecfence_ws/src/findwheel/scripts/setting.prototxt");
+    // FenceController fc("/home/zx/zzw/catkin_ws/src/findwheel/scripts/setting.prototxt");
     if(fc.m_initialized) {
 //
 //        ::ros::NodeHandle g_ros_handle;

src/findwheel/src/region_detect.cpp

@@ -1,7 +1,7 @@
 //
 // Created by zx on 2019/12/6.
 //
-#include "include/region_detect.h"
+#include "region_detect.h"
 
 Region_detector::Region_detector(int id, EleFence::Region region):m_region_id(-1)
 {
@@ -45,8 +45,8 @@ bool Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, p
     // 离群点过滤
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_out);
-    sor.setMeanK(10); //K近邻搜索点个数
-    sor.setStddevMulThresh(4.0); //标准差倍数
+    sor.setMeanK(15); //K近邻搜索点个数
+    sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false); //保留未滤波点（内点）
     sor.filter(*cloud_out);  //保存滤波结果到cloud_filter
 
@@ -86,7 +86,7 @@ bool Region_detector::isRect(std::vector<cv::Point2f>& points)
             }
         }
         double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
-        if (fabs(cosa) >= 0.13) {
+        if (fabs(cosa) >= 0.15 || std::min(l1,l2)>2.0 || std::max(l1,l2)>3.3) {
             std::cout << " angle cos >0.13 =" << cosa << "  i=" << max_index;
             std::cout << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
             return false;
@@ -104,7 +104,7 @@ bool Region_detector::isRect(std::vector<cv::Point2f>& points)
         double d = distance(pc, points[3]);
         cv::Point2f center1 = (ps + pt) * 0.5;
         cv::Point2f center2 = (pc + points[3]) * 0.5;
-        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
+        if (fabs(d - max_l) > max_l * 0.15 || distance(center1, center2) > 0.150) {
             std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2
                     << "  center distance=" << distance(center1, center2);;
             return false;
@@ -137,7 +137,7 @@ bool Region_detector::isRect(std::vector<cv::Point2f>& points)
             }
         }
         double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
-        if (fabs(cosa) >= 0.12) {
+        if (fabs(cosa) >= 0.15) {
             std::cout << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
             std::cout << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
             return false;
@@ -284,13 +284,8 @@ bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, doubl
         double dist0=0, dist1=0, dist2=0;
         wheelbase = std::max(wheel_box.size.width, wheel_box.size.height);
         cv::RotatedRect wheel_center_box = cv::minAreaRect(corner_points);
-        cv::Point2f center_vertice[4];
-        wheel_box.points(center_vertice);
-        len1 = pow(center_vertice[0].x - center_vertice[1].x, 2.0) + pow(center_vertice[0].y - center_vertice[1].y, 2.0);
-        len2 = pow(center_vertice[1].x - center_vertice[2].x, 2.0) + pow(center_vertice[1].y - center_vertice[2].y, 2.0);
-        if(len1 > 0 && len2 > 0){
-            wheelbase = std::max(len1, len2);
-        }
+        wheelbase = std::max(wheel_center_box.size.width, wheel_center_box.size.height);
+
         width = std::min(wheel_box.size.width, wheel_box.size.height);
         std::cout<<"--------detector find all------"<<std::endl;
     }

src/findwheel/src/include/region_detect.h

@@ -39,8 +39,8 @@ using google::protobuf::io::ZeroCopyOutputStream;
 using google::protobuf::io::CodedOutputStream;
 using google::protobuf::Message;
 
-#include "EleFence.pb.h"
-#include "StdCondition.h"
+#include "tools/EleFence.pb.h"
+#include "tools/StdCondition.h"
 #include "opencv2/opencv.hpp"
 
 class Region_detector

src/findwheel/src/region_worker.cpp

@@ -1,8 +1,8 @@
 //
 // Created by zx on 2019/12/9.
 //
-#include "include/region_worker.h"
-#include "include/PlcData.h"
+#include "region_worker.h"
+#include "PlcData.h"
 
 Region_worker::Region_worker(int id, EleFence::Region region){
     m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

src/findwheel/src/include/region_worker.h

@@ -6,11 +6,11 @@
 #define REGION_WORKER_H
 
 #include "region_detect.h"
-#include "StdCondition.h"
+#include "tools/StdCondition.h"
 #include <thread>
 #include <mutex>
 #include <iostream>
-#include "define.h"
+#include "tools/define.h"
 
 class Region_worker{
 public:

src/findwheel/src/EleFence.pb.cc

@@ -1,7 +1,7 @@
 // Generated by the protocol buffer compiler.  DO NOT EDIT!
 // source: EleFence.proto
 
-#include "include/EleFence.pb.h"
+#include "EleFence.pb.h"
 
 #include <algorithm>
 
@@ -170,8 +170,16 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::lidarParam, topic_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::lidarParam, transform_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::lidarParam, dist_limit_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::lidarParam, minx_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::lidarParam, maxx_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::lidarParam, miny_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::lidarParam, maxy_),
   0,
   1,
+  3,
+  4,
+  5,
+  6,
   2,
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::globalParam, _has_bits_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::EleFence::globalParam, _internal_metadata_),
@@ -186,8 +194,8 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
 static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
   { 0, 9, sizeof(::EleFence::Region)},
   { 13, 24, sizeof(::EleFence::Transform2d)},
-  { 30, 38, sizeof(::EleFence::lidarParam)},
-  { 41, 48, sizeof(::EleFence::globalParam)},
+  { 30, 42, sizeof(::EleFence::lidarParam)},
+  { 49, 56, sizeof(::EleFence::globalParam)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
@@ -224,14 +232,16 @@ void AddDescriptorsImpl() {
       "\n\004maxy\030\004 \002(\002\"m\n\013Transform2d\022\016\n\003m00\030\001 \001(\002"
       ":\0011\022\016\n\003m01\030\002 \001(\002:\0010\022\016\n\003m02\030\003 \001(\002:\0010\022\016\n\003m"
       "10\030\004 \001(\002:\0010\022\016\n\003m11\030\005 \001(\002:\0011\022\016\n\003m12\030\006 \001(\002"
-      ":\0010\"\\\n\nlidarParam\022\r\n\005topic\030\001 \001(\t\022(\n\ttran"
-      "sform\030\002 \001(\0132\025.EleFence.Transform2d\022\025\n\ndi"
-      "st_limit\030\003 \001(\002:\0018\"\\\n\013globalParam\022*\n\014lida"
-      "r_params\030\001 \003(\0132\024.EleFence.lidarParam\022!\n\007"
-      "regions\030\002 \003(\0132\020.EleFence.Region"
+      ":\0010\"\251\001\n\nlidarParam\022\r\n\005topic\030\001 \001(\t\022(\n\ttra"
+      "nsform\030\002 \001(\0132\025.EleFence.Transform2d\022\025\n\nd"
+      "ist_limit\030\003 \001(\002:\0018\022\020\n\004minx\030\004 \001(\002:\002-6\022\022\n\004"
+      "maxx\030\005 \001(\002:\004-0.2\022\022\n\004miny\030\006 \001(\002:\004-3.5\022\021\n\004"
+      "maxy\030\007 \001(\002:\0033.5\"\\\n\013globalParam\022*\n\014lidar_"
+      "params\030\001 \003(\0132\024.EleFence.lidarParam\022!\n\007re"
+      "gions\030\002 \003(\0132\020.EleFence.Region"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 391);
+      descriptor, 469);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "EleFence.proto", &protobuf_RegisterTypes);
 }
@@ -1068,6 +1078,10 @@ void lidarParam::InitAsDefaultInstance() {
 const int lidarParam::kTopicFieldNumber;
 const int lidarParam::kTransformFieldNumber;
 const int lidarParam::kDistLimitFieldNumber;
+const int lidarParam::kMinxFieldNumber;
+const int lidarParam::kMaxxFieldNumber;
+const int lidarParam::kMinyFieldNumber;
+const int lidarParam::kMaxyFieldNumber;
 #endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
 
 lidarParam::lidarParam()
@@ -1093,7 +1107,9 @@ lidarParam::lidarParam(const lidarParam& from)
   } else {
     transform_ = NULL;
   }
-  dist_limit_ = from.dist_limit_;
+  ::memcpy(&maxy_, &from.maxy_,
+    static_cast<size_t>(reinterpret_cast<char*>(&miny_) -
+    reinterpret_cast<char*>(&maxy_)) + sizeof(miny_));
   // @@protoc_insertion_point(copy_constructor:EleFence.lidarParam)
 }
 
@@ -1101,7 +1117,11 @@ void lidarParam::SharedCtor() {
   _cached_size_ = 0;
   topic_.UnsafeSetDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
   transform_ = NULL;
+  maxy_ = 3.5f;
   dist_limit_ = 8;
+  minx_ = -6;
+  maxx_ = -0.2f;
+  miny_ = -3.5f;
 }
 
 lidarParam::~lidarParam() {
@@ -1144,7 +1164,7 @@ void lidarParam::Clear() {
   (void) cached_has_bits;
 
   cached_has_bits = _has_bits_[0];
-  if (cached_has_bits & 7u) {
+  if (cached_has_bits & 127u) {
     if (cached_has_bits & 0x00000001u) {
       GOOGLE_DCHECK(!topic_.IsDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited()));
       (*topic_.UnsafeRawStringPointer())->clear();
@@ -1153,7 +1173,11 @@ void lidarParam::Clear() {
       GOOGLE_DCHECK(transform_ != NULL);
       transform_->Clear();
     }
+    maxy_ = 3.5f;
     dist_limit_ = 8;
+    minx_ = -6;
+    maxx_ = -0.2f;
+    miny_ = -3.5f;
   }
   _has_bits_.Clear();
   _internal_metadata_.Clear();
@@ -1211,6 +1235,62 @@ bool lidarParam::MergePartialFromCodedStream(
         break;
       }
 
+      // optional float minx = 4 [default = -6];
+      case 4: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(37u /* 37 & 0xFF */)) {
+          set_has_minx();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &minx_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // optional float maxx = 5 [default = -0.2];
+      case 5: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(45u /* 45 & 0xFF */)) {
+          set_has_maxx();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &maxx_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // optional float miny = 6 [default = -3.5];
+      case 6: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(53u /* 53 & 0xFF */)) {
+          set_has_miny();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &miny_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // optional float maxy = 7 [default = 3.5];
+      case 7: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(61u /* 61 & 0xFF */)) {
+          set_has_maxy();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &maxy_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
       default: {
       handle_unusual:
         if (tag == 0) {
@@ -1255,10 +1335,30 @@ void lidarParam::SerializeWithCachedSizes(
   }
 
   // optional float dist_limit = 3 [default = 8];
-  if (cached_has_bits & 0x00000004u) {
+  if (cached_has_bits & 0x00000008u) {
     ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->dist_limit(), output);
   }
 
+  // optional float minx = 4 [default = -6];
+  if (cached_has_bits & 0x00000010u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->minx(), output);
+  }
+
+  // optional float maxx = 5 [default = -0.2];
+  if (cached_has_bits & 0x00000020u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(5, this->maxx(), output);
+  }
+
+  // optional float miny = 6 [default = -3.5];
+  if (cached_has_bits & 0x00000040u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(6, this->miny(), output);
+  }
+
+  // optional float maxy = 7 [default = 3.5];
+  if (cached_has_bits & 0x00000004u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(7, this->maxy(), output);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
         _internal_metadata_.unknown_fields(), output);
@@ -1293,10 +1393,30 @@ void lidarParam::SerializeWithCachedSizes(
   }
 
   // optional float dist_limit = 3 [default = 8];
-  if (cached_has_bits & 0x00000004u) {
+  if (cached_has_bits & 0x00000008u) {
     target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->dist_limit(), target);
   }
 
+  // optional float minx = 4 [default = -6];
+  if (cached_has_bits & 0x00000010u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->minx(), target);
+  }
+
+  // optional float maxx = 5 [default = -0.2];
+  if (cached_has_bits & 0x00000020u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(5, this->maxx(), target);
+  }
+
+  // optional float miny = 6 [default = -3.5];
+  if (cached_has_bits & 0x00000040u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(6, this->miny(), target);
+  }
+
+  // optional float maxy = 7 [default = 3.5];
+  if (cached_has_bits & 0x00000004u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(7, this->maxy(), target);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields(), target);
@@ -1314,7 +1434,7 @@ size_t lidarParam::ByteSizeLong() const {
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
         _internal_metadata_.unknown_fields());
   }
-  if (_has_bits_[0 / 32] & 7u) {
+  if (_has_bits_[0 / 32] & 127u) {
     // optional string topic = 1;
     if (has_topic()) {
       total_size += 1 +
@@ -1329,11 +1449,31 @@ size_t lidarParam::ByteSizeLong() const {
           *this->transform_);
     }
 
+    // optional float maxy = 7 [default = 3.5];
+    if (has_maxy()) {
+      total_size += 1 + 4;
+    }
+
     // optional float dist_limit = 3 [default = 8];
     if (has_dist_limit()) {
       total_size += 1 + 4;
     }
 
+    // optional float minx = 4 [default = -6];
+    if (has_minx()) {
+      total_size += 1 + 4;
+    }
+
+    // optional float maxx = 5 [default = -0.2];
+    if (has_maxx()) {
+      total_size += 1 + 4;
+    }
+
+    // optional float miny = 6 [default = -3.5];
+    if (has_miny()) {
+      total_size += 1 + 4;
+    }
+
   }
   int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
@@ -1365,7 +1505,7 @@ void lidarParam::MergeFrom(const lidarParam& from) {
   (void) cached_has_bits;
 
   cached_has_bits = from._has_bits_[0];
-  if (cached_has_bits & 7u) {
+  if (cached_has_bits & 127u) {
     if (cached_has_bits & 0x00000001u) {
       set_has_topic();
       topic_.AssignWithDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), from.topic_);
@@ -1374,8 +1514,20 @@ void lidarParam::MergeFrom(const lidarParam& from) {
       mutable_transform()->::EleFence::Transform2d::MergeFrom(from.transform());
     }
     if (cached_has_bits & 0x00000004u) {
+      maxy_ = from.maxy_;
+    }
+    if (cached_has_bits & 0x00000008u) {
       dist_limit_ = from.dist_limit_;
     }
+    if (cached_has_bits & 0x00000010u) {
+      minx_ = from.minx_;
+    }
+    if (cached_has_bits & 0x00000020u) {
+      maxx_ = from.maxx_;
+    }
+    if (cached_has_bits & 0x00000040u) {
+      miny_ = from.miny_;
+    }
     _has_bits_[0] |= cached_has_bits;
   }
 }
@@ -1406,7 +1558,11 @@ void lidarParam::InternalSwap(lidarParam* other) {
   using std::swap;
   topic_.Swap(&other->topic_);
   swap(transform_, other->transform_);
+  swap(maxy_, other->maxy_);
   swap(dist_limit_, other->dist_limit_);
+  swap(minx_, other->minx_);
+  swap(maxx_, other->maxx_);
+  swap(miny_, other->miny_);
   swap(_has_bits_[0], other->_has_bits_[0]);
   _internal_metadata_.Swap(&other->_internal_metadata_);
   swap(_cached_size_, other->_cached_size_);

src/findwheel/src/include/EleFence.pb.h

@@ -491,6 +491,13 @@ class lidarParam : public ::google::protobuf::Message /* @@protoc_insertion_poin
   ::EleFence::Transform2d* mutable_transform();
   void set_allocated_transform(::EleFence::Transform2d* transform);
 
+  // optional float maxy = 7 [default = 3.5];
+  bool has_maxy() const;
+  void clear_maxy();
+  static const int kMaxyFieldNumber = 7;
+  float maxy() const;
+  void set_maxy(float value);
+
   // optional float dist_limit = 3 [default = 8];
   bool has_dist_limit() const;
   void clear_dist_limit();
@@ -498,6 +505,27 @@ class lidarParam : public ::google::protobuf::Message /* @@protoc_insertion_poin
   float dist_limit() const;
   void set_dist_limit(float value);
 
+  // optional float minx = 4 [default = -6];
+  bool has_minx() const;
+  void clear_minx();
+  static const int kMinxFieldNumber = 4;
+  float minx() const;
+  void set_minx(float value);
+
+  // optional float maxx = 5 [default = -0.2];
+  bool has_maxx() const;
+  void clear_maxx();
+  static const int kMaxxFieldNumber = 5;
+  float maxx() const;
+  void set_maxx(float value);
+
+  // optional float miny = 6 [default = -3.5];
+  bool has_miny() const;
+  void clear_miny();
+  static const int kMinyFieldNumber = 6;
+  float miny() const;
+  void set_miny(float value);
+
   // @@protoc_insertion_point(class_scope:EleFence.lidarParam)
  private:
   void set_has_topic();
@@ -506,13 +534,25 @@ class lidarParam : public ::google::protobuf::Message /* @@protoc_insertion_poin
   void clear_has_transform();
   void set_has_dist_limit();
   void clear_has_dist_limit();
+  void set_has_minx();
+  void clear_has_minx();
+  void set_has_maxx();
+  void clear_has_maxx();
+  void set_has_miny();
+  void clear_has_miny();
+  void set_has_maxy();
+  void clear_has_maxy();
 
   ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
   ::google::protobuf::internal::HasBits<1> _has_bits_;
   mutable int _cached_size_;
   ::google::protobuf::internal::ArenaStringPtr topic_;
   ::EleFence::Transform2d* transform_;
+  float maxy_;
   float dist_limit_;
+  float minx_;
+  float maxx_;
+  float miny_;
   friend struct ::protobuf_EleFence_2eproto::TableStruct;
   friend void ::protobuf_EleFence_2eproto::InitDefaultslidarParamImpl();
 };
@@ -1020,13 +1060,13 @@ inline void lidarParam::set_allocated_transform(::EleFence::Transform2d* transfo
 
 // optional float dist_limit = 3 [default = 8];
 inline bool lidarParam::has_dist_limit() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
+  return (_has_bits_[0] & 0x00000008u) != 0;
 }
 inline void lidarParam::set_has_dist_limit() {
-  _has_bits_[0] |= 0x00000004u;
+  _has_bits_[0] |= 0x00000008u;
 }
 inline void lidarParam::clear_has_dist_limit() {
-  _has_bits_[0] &= ~0x00000004u;
+  _has_bits_[0] &= ~0x00000008u;
 }
 inline void lidarParam::clear_dist_limit() {
   dist_limit_ = 8;
@@ -1042,6 +1082,102 @@ inline void lidarParam::set_dist_limit(float value) {
   // @@protoc_insertion_point(field_set:EleFence.lidarParam.dist_limit)
 }
 
+// optional float minx = 4 [default = -6];
+inline bool lidarParam::has_minx() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
+}
+inline void lidarParam::set_has_minx() {
+  _has_bits_[0] |= 0x00000010u;
+}
+inline void lidarParam::clear_has_minx() {
+  _has_bits_[0] &= ~0x00000010u;
+}
+inline void lidarParam::clear_minx() {
+  minx_ = -6;
+  clear_has_minx();
+}
+inline float lidarParam::minx() const {
+  // @@protoc_insertion_point(field_get:EleFence.lidarParam.minx)
+  return minx_;
+}
+inline void lidarParam::set_minx(float value) {
+  set_has_minx();
+  minx_ = value;
+  // @@protoc_insertion_point(field_set:EleFence.lidarParam.minx)
+}
+
+// optional float maxx = 5 [default = -0.2];
+inline bool lidarParam::has_maxx() const {
+  return (_has_bits_[0] & 0x00000020u) != 0;
+}
+inline void lidarParam::set_has_maxx() {
+  _has_bits_[0] |= 0x00000020u;
+}
+inline void lidarParam::clear_has_maxx() {
+  _has_bits_[0] &= ~0x00000020u;
+}
+inline void lidarParam::clear_maxx() {
+  maxx_ = -0.2f;
+  clear_has_maxx();
+}
+inline float lidarParam::maxx() const {
+  // @@protoc_insertion_point(field_get:EleFence.lidarParam.maxx)
+  return maxx_;
+}
+inline void lidarParam::set_maxx(float value) {
+  set_has_maxx();
+  maxx_ = value;
+  // @@protoc_insertion_point(field_set:EleFence.lidarParam.maxx)
+}
+
+// optional float miny = 6 [default = -3.5];
+inline bool lidarParam::has_miny() const {
+  return (_has_bits_[0] & 0x00000040u) != 0;
+}
+inline void lidarParam::set_has_miny() {
+  _has_bits_[0] |= 0x00000040u;
+}
+inline void lidarParam::clear_has_miny() {
+  _has_bits_[0] &= ~0x00000040u;
+}
+inline void lidarParam::clear_miny() {
+  miny_ = -3.5f;
+  clear_has_miny();
+}
+inline float lidarParam::miny() const {
+  // @@protoc_insertion_point(field_get:EleFence.lidarParam.miny)
+  return miny_;
+}
+inline void lidarParam::set_miny(float value) {
+  set_has_miny();
+  miny_ = value;
+  // @@protoc_insertion_point(field_set:EleFence.lidarParam.miny)
+}
+
+// optional float maxy = 7 [default = 3.5];
+inline bool lidarParam::has_maxy() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+inline void lidarParam::set_has_maxy() {
+  _has_bits_[0] |= 0x00000004u;
+}
+inline void lidarParam::clear_has_maxy() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+inline void lidarParam::clear_maxy() {
+  maxy_ = 3.5f;
+  clear_has_maxy();
+}
+inline float lidarParam::maxy() const {
+  // @@protoc_insertion_point(field_get:EleFence.lidarParam.maxy)
+  return maxy_;
+}
+inline void lidarParam::set_maxy(float value) {
+  set_has_maxy();
+  maxy_ = value;
+  // @@protoc_insertion_point(field_set:EleFence.lidarParam.maxy)
+}
+
 // -------------------------------------------------------------------
 
 // globalParam

src/findwheel/src/LogFiles.cpp

@@ -1,5 +1,5 @@
 
-#include "include/LogFiles.h"
+#include "LogFiles.h"
 #include <string.h>
 
 const string CLogFiles::m_strFileNameLoglaserBinary1 = "laser1.data";

src/findwheel/src/StdCondition.cpp

@@ -1,4 +1,4 @@
-#include "include/StdCondition.h"
+#include "StdCondition.h"
 
 StdCondition::StdCondition():m_value(false)
 {

src/findwheel/src/globalmsg.pb.cc

@@ -1,7 +1,7 @@
 // Generated by the protocol buffer compiler.  DO NOT EDIT!
 // source: globalmsg.proto
 
-#include "include/globalmsg.pb.h"
+#include "globalmsg.pb.h"
 
 #include <algorithm>
 

src/findwheel/src/s7_plc.cpp

@@ -1,4 +1,4 @@
-#include "include/s7_plc.h"
+#include "s7_plc.h"
 
 S7PLC::S7PLC():bConnected_(false)
 {