add enumerator stateCode. open interval check function.

.idea/vcs.xml

@@ -2,5 +2,6 @@
 <project version="4">
   <component name="VcsDirectoryMappings">
     <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+    <mapping directory="$PROJECT_DIR$" vcs="Git" />
   </component>
 </project>

.idea/workspace.xml

@@ -2,61 +2,61 @@
 <project version="4">
   <component name="CMakeRunConfigurationManager" shouldGenerate="true" shouldDeleteObsolete="true">
     <generated>
-      <config projectName="FindWheel" targetName="tf_generate_messages_lisp" />
-      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_cpp" />
-      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_py" />
-      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_eus" />
-      <config projectName="FindWheel" targetName="tests" />
-      <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_cpp" />
-      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_py" />
-      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_lisp" />
-      <config projectName="FindWheel" targetName="roscpp_generate_messages_py" />
-      <config projectName="FindWheel" targetName="actionlib_generate_messages_cpp" />
-      <config projectName="FindWheel" targetName="gmock" />
-      <config projectName="FindWheel" targetName="doxygen" />
-      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_cpp" />
-      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="actionlib_generate_messages_py" />
-      <config projectName="FindWheel" targetName="tf_generate_messages_eus" />
-      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_py" />
-      <config projectName="FindWheel" targetName="roscpp_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="std_msgs_generate_messages_lisp" />
-      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_lisp" />
-      <config projectName="FindWheel" targetName="run_tests" />
-      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_cpp" />
       <config projectName="FindWheel" targetName="tf_generate_messages_py" />
-      <config projectName="FindWheel" targetName="std_msgs_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="clean_test_results" />
-      <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_py" />
-      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_lisp" />
-      <config projectName="FindWheel" targetName="std_msgs_generate_messages_py" />
-      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="FindWheel_node" />
-      <config projectName="FindWheel" targetName="std_msgs_generate_messages_eus" />
       <config projectName="FindWheel" targetName="roscpp_generate_messages_cpp" />
-      <config projectName="FindWheel" targetName="tf_generate_messages_cpp" />
-      <config projectName="FindWheel" targetName="gmock_main" />
+      <config projectName="FindWheel" targetName="std_msgs_generate_messages_lisp" />
+      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_lisp" />
       <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_py" />
-      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_eus" />
-      <config projectName="FindWheel" targetName="gtest" />
-      <config projectName="FindWheel" targetName="gtest_main" />
-      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_eus" />
       <config projectName="FindWheel" targetName="actionlib_generate_messages_lisp" />
       <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_eus" />
       <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_lisp" />
+      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_nodejs" />
+      <config projectName="FindWheel" targetName="actionlib_generate_messages_cpp" />
       <config projectName="FindWheel" targetName="tf_generate_messages_nodejs" />
-      <config projectName="FindWheel" targetName="actionlib_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="gmock_main" />
       <config projectName="FindWheel" targetName="download_extra_data" />
+      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_py" />
+      <config projectName="FindWheel" targetName="tests" />
+      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_py" />
+      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_nodejs" />
+      <config projectName="FindWheel" targetName="gtest_main" />
+      <config projectName="FindWheel" targetName="run_tests" />
+      <config projectName="FindWheel" targetName="doxygen" />
+      <config projectName="FindWheel" targetName="gtest" />
+      <config projectName="FindWheel" targetName="roscpp_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_py" />
+      <config projectName="FindWheel" targetName="clean_test_results" />
+      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_cpp" />
+      <config projectName="FindWheel" targetName="actionlib_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="std_msgs_generate_messages_cpp" />
+      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_lisp" />
+      <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_py" />
+      <config projectName="FindWheel" targetName="std_msgs_generate_messages_py" />
+      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="actionlib_generate_messages_py" />
+      <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_nodejs" />
+      <config projectName="FindWheel" targetName="roscpp_generate_messages_nodejs" />
+      <config projectName="FindWheel" targetName="FindWheel_node" />
       <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_eus" />
-      <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_lisp" />
+      <config projectName="FindWheel" targetName="gmock" />
+      <config projectName="FindWheel" targetName="tf_generate_messages_cpp" />
       <config projectName="FindWheel" targetName="actionlib_generate_messages_nodejs" />
+      <config projectName="FindWheel" targetName="geometry_msgs_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="std_msgs_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_lisp" />
       <config projectName="FindWheel" targetName="tf2_msgs_generate_messages_cpp" />
+      <config projectName="FindWheel" targetName="roscpp_generate_messages_py" />
+      <config projectName="FindWheel" targetName="std_msgs_generate_messages_nodejs" />
+      <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_lisp" />
+      <config projectName="FindWheel" targetName="rosgraph_msgs_generate_messages_cpp" />
+      <config projectName="FindWheel" targetName="tf_generate_messages_eus" />
+      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_cpp" />
+      <config projectName="FindWheel" targetName="sensor_msgs_generate_messages_nodejs" />
+      <config projectName="FindWheel" targetName="actionlib_msgs_generate_messages_cpp" />
+      <config projectName="FindWheel" targetName="tf_generate_messages_lisp" />
       <config projectName="FindWheel" targetName="roscpp_generate_messages_lisp" />
-      <config projectName="FindWheel" targetName="std_msgs_generate_messages_cpp" />
-      <config projectName="FindWheel" targetName="roscpp_generate_messages_eus" />
     </generated>
   </component>
   <component name="CMakeSettings">
@@ -66,33 +66,17 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="db26adf3-25e5-468c-a9bf-e3e4e303610a" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/bag/in.bag" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/bag/in_noise.bag" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/bag/out.bag" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/cloud_1.txt" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/cloud_2.txt" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/scripts/EleFence.proto" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/scripts/setting.prototxt" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/EleFence.pb.cc" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/EleFence.pb.h" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/FenceController.cpp" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/FenceController.h" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/Lidar.cpp" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/Lidar.h" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/LogFiles.cpp" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/LogFiles.h" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/PlcData.cpp" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/PlcData.h" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/region_detect.cpp" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/region_detect.h" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/region_worker.cpp" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/src/region_worker.h" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/src/define.h" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/../.idea/misc.xml" beforeDir="false" afterPath="$PROJECT_DIR$/../.idea/misc.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/vcs.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/vcs.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/CMakeLists.txt" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/package.xml" beforeDir="false" afterPath="$PROJECT_DIR$/package.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/src/find_wheel.cpp" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/src/find_wheel.h" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/src/find_wheel_node.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/find_wheel_node.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/FenceController.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/FenceController.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/Lidar.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/Lidar.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/PlcData.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/PlcData.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/PlcData.h" beforeDir="false" afterPath="$PROJECT_DIR$/src/PlcData.h" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/region_worker.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/region_worker.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/region_worker.h" beforeDir="false" afterPath="$PROJECT_DIR$/src/region_worker.h" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/../MPC/CMakeLists.txt" beforeDir="false" />
       <change beforePath="$PROJECT_DIR$/../MPC/launch/MPC.launch" beforeDir="false" />
       <change beforePath="$PROJECT_DIR$/../MPC/launch/MPC_2.launch" beforeDir="false" />
@@ -898,7 +882,7 @@
   </component>
   <component name="ExecutionTargetManager" SELECTED_TARGET="CMakeBuildProfile:Debug" />
   <component name="Git.Settings">
-    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$/.." />
+    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
   </component>
   <component name="ProjectId" id="1UjfYrLi8M0cZqyvMMzek8Cxnic" />
   <component name="ProjectLevelVcsManager" settingsEditedManually="true" />
@@ -1263,7 +1247,7 @@
       <updated>1575874975400</updated>
       <workItem from="1575874977165" duration="14000" />
       <workItem from="1575875076982" duration="22712000" />
-      <workItem from="1576028909213" duration="5515000" />
+      <workItem from="1576028909213" duration="7647000" />
     </task>
     <servers />
   </component>

CMakeLists.txt

@@ -152,7 +152,7 @@ add_executable(${PROJECT_NAME}_node
     src/StdCondition.cpp
     src/region_worker.cpp
     src/s7_plc.cpp
-        src/PlcData.cpp src/PlcData.h)
+        src/PlcData.cpp src/PlcData.h src/define.h)
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} snap7)
 
 ## Rename C++ executable without prefix

src/FenceController.cpp

@@ -79,8 +79,10 @@ void FenceController::cloudMergeUpdate(FenceController* fc)
                 fc->p_merged_cloud->operator+=(*cloud);
             }
         }
-        for (int i = 0; i < fc->p_region_workers.size(); ++i) {
-            fc->p_region_workers[i]->update_cloud(fc->p_merged_cloud);
+        if(fc->p_merged_cloud->size() > 0) {
+            for (int i = 0; i < fc->p_region_workers.size(); ++i) {
+                fc->p_region_workers[i]->update_cloud(fc->p_merged_cloud);
+            }
         }
         fc->m_cloud_mutex.unlock();
     }

src/Lidar.cpp

@@ -95,7 +95,7 @@ bool Lidar::GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int timeout_mil
 {
     cloud=m_cloud;
 //    std::cout<<cloud->size()<<std::endl;
-    return true;
+//    return true;
     ros::Duration duration = ros::Time::now() - m_cloud_time;
     if(duration.toSec()*1000 > timeout_milli)
         return false;

src/PlcData.cpp

@@ -14,15 +14,30 @@ PlcData* PlcData::GetInstance(std::string ip)
     return g_instance;
 
 }
-void PlcData::UpdateData(short* data,int id)
+void PlcData::UpdateData(stateCode code,int id)
 {
     const int length=1;
     int offset=id*length;
+    short* data = (short*)malloc(sizeof(short));
+    memset(data, 0, sizeof(short));
+    switch(code){
+        case stateCode::eOk:
+            *data = (short)1;
+            break;
+        case stateCode::eOutOfPlace:
+            *data = (short)2;
+            break;
+        case stateCode::eNoCar:
+        default:
+            *data = (short)0;
+            break;
+    }
 //    printf("data: %d \n", *data);
     m_lock.lock();
     ////判断越界
     if(offset>=0 && offset+length<=MAX_REGIONS)
         memcpy(m_data+offset,data,length*sizeof(short));
+    free(data);
     m_lock.unlock();
 }
 

src/PlcData.h

@@ -11,6 +11,8 @@
 #include <thread>
 #include "s7_plc.h"
 #include "unistd.h"
+#include "define.h"
+
 #define MAX_REGIONS 15
 #define ELE_FENCE_START_ADDR 0
 #define ELE_FENCE_DB_NUM 48
@@ -20,7 +22,7 @@ class PlcData
 public:
     static PlcData* GetInstance(std::string ip="");
     static void Release();
-    void UpdateData(short* data,int id);
+    void UpdateData(stateCode code,int id);
     static void plcThread(PlcData* p);
 private:
     static PlcData*                                 g_instance;

src/define.h

@@ -0,0 +1,14 @@
+//
+// Created by zx on 2019/12/11.
+//
+
+#ifndef DEFINE_H
+#define DEFINE_H
+
+enum stateCode{
+    eNoCar=0,
+    eOk,
+    eOutOfPlace
+};
+
+#endif //DEFINE_H

src/region_worker.cpp

@@ -39,20 +39,21 @@ void Region_worker::detect_loop(Region_worker *worker)
     if(worker == 0) return;
     if(worker->m_detector == 0) return;
     while(!worker->m_cond_exit.WaitFor(1)){
-        short *p_data;
-        p_data = (short *)malloc(sizeof(short));
-        memset(p_data, 0, sizeof(short));
+        stateCode code = stateCode::eNoCar;
         worker->m_mutex.lock();
         if(worker->b_cloud_updated){
             bool result = worker->m_detector->detect(worker->m_cloud);
             worker->b_cloud_updated = false;
-            *p_data = result? (short)1: (short)2;
+            if(result){
+                code = stateCode ::eOk;
+            }else{
+                code = stateCode ::eOutOfPlace;
+            }
         }
         PlcData* p = PlcData::GetInstance();
         if(p) {
-            p->UpdateData(p_data, worker->m_detector->m_region_id);
+            p->UpdateData(code, worker->m_detector->m_region_id);
         }
-        free(p_data);
         worker->m_mutex.unlock();
     }
 }

src/region_worker.h

@@ -10,6 +10,7 @@
 #include <thread>
 #include <mutex>
 #include <iostream>
+#include "define.h"
 
 class Region_worker{
 public: