electric Fence encapsulation.

.gitignore

@@ -0,0 +1,30 @@
+
+#ignore thumbnails created by windows
+Thumbs.db
+#Ignore files build by Visual Studio
+*.obj
+*.exe
+*.pdb
+*.user
+*.aps
+*.pch
+*.vspscc
+*_i.c
+*_p.c
+*.ncb
+*.suo
+*.tlb
+*.tlh
+*.bak
+*.cache
+*.ilk
+*.log
+[Bb]in
+[Dd]ebug*/
+*.lib
+*.sbr
+obj/
+[Rr]elease*/
+_ReSharper*/
+[Tt]est[Rr]esult*
+cmake-build-debug/*

.idea/FindWheel.iml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module classpath="CMake" type="CPP_MODULE" version="4" />

.idea/misc.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
+  <component name="JavaScriptSettings">
+    <option name="languageLevel" value="ES6" />
+  </component>
+</project>

.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/FindWheel.iml" filepath="$PROJECT_DIR$/.idea/FindWheel.iml" />
+    </modules>
+  </component>
+</project>

.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

CMakeLists.txt

@@ -0,0 +1,219 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(FindWheel)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+FIND_PACKAGE(Protobuf REQUIRED)
+FIND_PACKAGE(Glog REQUIRED)
+set(CMAKE_MODULE_PATH "/usr/local/share/")
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  tf
+  sensor_msgs
+)
+include_directories("/usr/include/eigen3")
+find_package(PCL REQUIRED)
+include_directories(include ${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES FindWheel
+#  CATKIN_DEPENDS roscpp std::msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+        /usr/local/include/snap7
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/FindWheel.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node 
+    src/find_wheel_node.cpp
+    src/LogFiles.cpp
+    src/Lidar.cpp
+    src/region_detect.cpp
+    src/FenceController.cpp
+    src/EleFence.pb.cc
+    src/StdCondition.cpp
+    src/region_worker.cpp
+    src/s7_plc.cpp
+        src/PlcData.cpp src/PlcData.h)
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} snap7)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_FindWheel.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

cloud_1.txt

@@ -0,0 +1,683 @@
+1.086 6.089 0.000
+1.099 6.090 0.000
+1.111 6.090 0.000
+1.123 6.090 0.000
+1.136 6.091 0.000
+1.148 6.090 0.000
+1.160 6.090 0.000
+1.173 6.090 0.000
+1.184 6.089 0.000
+1.197 6.089 0.000
+1.209 6.088 0.000
+1.222 6.088 0.000
+1.236 6.088 0.000
+1.251 6.089 0.000
+1.263 6.088 0.000
+1.278 6.088 0.000
+1.291 6.087 0.000
+1.304 6.086 0.000
+1.319 6.087 0.000
+1.338 6.090 0.000
+1.354 6.091 0.000
+1.369 6.091 0.000
+1.384 6.090 0.000
+1.402 6.093 0.000
+1.416 6.091 0.000
+1.433 6.092 0.000
+1.451 6.093 0.000
+1.466 6.092 0.000
+1.480 6.089 0.000
+1.448 6.053 0.000
+1.295 5.931 0.000
+1.320 5.939 0.000
+1.482 6.041 0.000
+1.563 6.085 0.000
+1.595 6.094 0.000
+1.615 6.095 0.000
+1.634 6.095 0.000
+1.653 6.095 0.000
+1.675 6.096 0.000
+1.696 6.096 0.000
+1.717 6.096 0.000
+1.740 6.097 0.000
+1.761 6.097 0.000
+1.783 6.097 0.000
+1.807 6.097 0.000
+1.831 6.098 0.000
+1.853 6.097 0.000
+1.877 6.098 0.000
+1.901 6.097 0.000
+1.925 6.097 0.000
+1.951 6.097 0.000
+1.976 6.096 0.000
+2.003 6.097 0.000
+2.028 6.096 0.000
+2.056 6.096 0.000
+2.091 6.100 0.000
+2.123 6.101 0.000
+2.155 6.103 0.000
+2.186 6.103 0.000
+1.931 5.963 0.000
+1.891 5.930 0.000
+2.165 6.047 0.000
+2.302 6.097 0.000
+2.339 6.098 0.000
+2.368 6.095 0.000
+2.404 6.096 0.000
+2.439 6.094 0.000
+2.479 6.095 0.000
+2.521 6.097 0.000
+2.560 6.096 0.000
+2.606 6.098 0.000
+2.648 6.098 0.000
+2.691 6.098 0.000
+2.738 6.099 0.000
+2.784 6.099 0.000
+2.833 6.100 0.000
+2.880 6.099 0.000
+2.935 6.100 0.000
+2.991 6.102 0.000
+3.056 6.105 0.000
+3.129 6.111 0.000
+3.178 6.108 0.000
+3.189 6.092 0.000
+3.191 6.072 0.000
+3.189 6.052 0.000
+3.187 6.031 0.000
+3.187 6.011 0.000
+3.186 5.991 0.000
+3.188 5.972 0.000
+3.193 5.954 0.000
+3.265 5.955 0.000
+3.937 6.096 0.000
+4.030 6.097 0.000
+4.131 6.098 0.000
+4.449 6.098 0.000
+4.574 6.101 0.000
+4.696 6.101 0.000
+4.829 6.102 0.000
+4.967 6.103 0.000
+5.113 6.103 0.000
+5.270 6.104 0.000
+5.417 6.102 0.000
+5.593 6.103 0.000
+5.775 6.103 0.000
+5.957 6.101 0.000
+2.880 5.521 0.000
+2.933 5.513 0.000
+2.941 5.497 0.000
+2.941 5.480 0.000
+2.971 5.467 0.000
+3.394 5.507 0.000
+3.491 5.501 0.000
+3.136 5.436 0.000
+2.992 5.401 0.000
+3.054 5.391 0.000
+3.664 5.417 0.000
+3.671 5.397 0.000
+3.673 5.376 0.000
+3.617 5.350 0.000
+3.248 5.301 0.000
+3.188 5.278 0.000
+3.205 5.261 0.000
+6.552 5.405 0.000
+6.531 5.366 0.000
+3.513 5.181 0.000
+3.457 5.160 0.000
+3.407 5.139 0.000
+3.361 5.119 0.000
+3.313 5.099 0.000
+3.278 5.080 0.000
+3.274 5.061 0.000
+3.283 5.042 0.000
+3.288 5.024 0.000
+3.299 5.005 0.000
+3.305 4.986 0.000
+3.312 4.967 0.000
+3.320 4.948 0.000
+3.327 4.929 0.000
+3.338 4.909 0.000
+3.347 4.890 0.000
+3.352 4.871 0.000
+3.363 4.851 0.000
+3.368 4.831 0.000
+3.386 4.811 0.000
+3.510 4.781 0.000
+5.834 4.527 0.000
+5.834 4.493 0.000
+5.840 4.459 0.000
+5.839 4.425 0.000
+5.841 4.391 0.000
+5.813 4.360 0.000
+5.758 4.334 0.000
+5.789 4.296 0.000
+5.831 4.256 0.000
+5.869 4.216 0.000
+5.902 4.177 0.000
+5.914 4.140 0.000
+5.929 4.103 0.000
+5.954 4.064 0.000
+5.993 4.022 0.000
+6.009 3.984 0.000
+6.022 3.946 0.000
+6.038 3.907 0.000
+6.035 3.871 0.000
+6.038 3.835 0.000
+6.057 3.794 0.000
+6.115 3.745 0.000
+6.089 3.714 0.000
+6.088 3.678 0.000
+6.103 3.637 0.000
+6.133 3.593 0.000
+6.236 3.530 0.000
+6.223 3.496 0.000
+6.216 3.459 0.000
+6.226 3.418 0.000
+6.255 3.372 0.000
+6.214 3.345 0.000
+6.198 3.311 0.000
+6.217 3.267 0.000
+6.398 3.174 0.000
+6.430 3.124 0.000
+6.406 3.091 0.000
+6.360 3.065 0.000
+6.339 3.032 0.000
+6.160 3.050 0.000
+6.269 2.975 0.000
+6.198 2.959 0.000
+6.276 2.891 0.000
+6.235 2.865 0.000
+6.152 2.855 0.000
+5.988 2.876 0.000
+5.981 2.839 0.000
+5.940 2.816 0.000
+5.929 2.781 0.000
+5.925 2.743 0.000
+5.964 2.688 0.000
+5.956 2.651 0.000
+5.989 2.598 0.000
+6.010 2.467 0.000
+2.422 4.001 0.000
+2.397 3.997 0.000
+2.367 3.994 0.000
+2.339 3.991 0.000
+2.319 3.984 0.000
+2.308 3.974 0.000
+2.301 3.961 0.000
+2.299 3.946 0.000
+2.296 3.932 0.000
+2.294 3.916 0.000
+2.296 3.899 0.000
+2.299 3.881 0.000
+2.302 3.862 0.000
+2.306 3.844 0.000
+2.312 3.824 0.000
+2.319 3.802 0.000
+2.324 3.782 0.000
+2.325 3.764 0.000
+2.324 3.748 0.000
+2.321 3.732 0.000
+2.314 3.719 0.000
+2.310 3.703 0.000
+2.304 3.689 0.000
+2.304 3.672 0.000
+2.300 3.655 0.000
+2.297 3.640 0.000
+2.292 3.625 0.000
+2.291 3.607 0.000
+2.290 3.589 0.000
+2.294 3.568 0.000
+5.582 1.214 0.000
+5.558 1.183 0.000
+5.888 0.137 0.000
+5.890 0.076 0.000
+5.893 0.015 0.000
+5.916 -0.065 0.000
+5.993 -0.381 0.000
+4.913 0.454 0.000
+4.833 0.477 0.000
+4.752 0.502 0.000
+4.678 0.522 0.000
+4.608 0.539 0.000
+4.575 0.520 0.000
+4.562 0.480 0.000
+4.549 0.440 0.000
+4.538 0.397 0.000
+4.524 0.357 0.000
+4.512 0.314 0.000
+4.501 0.271 0.000
+4.487 0.230 0.000
+1.035 3.987 0.000
+1.005 4.009 0.000
+0.995 4.008 0.000
+0.981 4.011 0.000
+0.969 4.014 0.000
+0.960 4.011 0.000
+0.946 4.016 0.000
+0.938 4.013 0.000
+0.927 4.014 0.000
+0.915 4.017 0.000
+0.907 4.015 0.000
+0.898 4.013 0.000
+0.887 4.014 0.000
+0.879 4.012 0.000
+0.871 4.011 0.000
+0.865 4.005 0.000
+0.859 4.002 0.000
+0.854 3.995 0.000
+0.850 3.987 0.000
+0.847 3.979 0.000
+0.844 3.969 0.000
+0.842 3.958 0.000
+0.841 3.946 0.000
+0.841 3.932 0.000
+0.843 3.915 0.000
+0.846 3.896 0.000
+0.851 3.874 0.000
+0.851 3.858 0.000
+0.849 3.847 0.000
+0.848 3.832 0.000
+0.847 3.818 0.000
+0.846 3.803 0.000
+0.845 3.788 0.000
+0.845 3.772 0.000
+0.849 3.747 0.000
+0.849 3.730 0.000
+0.848 3.713 0.000
+0.847 3.697 0.000
+0.847 3.678 0.000
+0.845 3.662 0.000
+0.844 3.645 0.000
+0.840 3.633 0.000
+0.837 3.618 0.000
+0.834 3.603 0.000
+0.833 3.585 0.000
+3.016 -0.860 0.000
+3.036 -0.988 0.000
+3.025 -1.056 0.000
+2.807 -3.478 0.000
+2.764 -3.517 0.000
+0.972 1.407 0.000
+0.948 1.411 0.000
+0.926 1.411 0.000
+0.903 1.412 0.000
+0.880 1.415 0.000
+0.858 1.413 0.000
+0.837 1.409 0.000
+0.816 1.404 0.000
+0.801 1.369 0.000
+0.800 1.254 0.000
+0.802 1.120 0.000
+0.790 1.051 0.000
+1.687 -5.427 0.000
+1.625 -5.432 0.000
+1.235 -5.847 0.000
+1.170 -5.834 0.000
+1.103 -5.805 0.000
+0.900 -4.886 0.000
+0.833 -4.777 0.000
+0.770 -4.703 0.000
+0.714 -4.706 0.000
+0.657 -4.714 0.000
+0.600 -4.707 0.000
+0.541 -4.681 0.000
+0.485 -4.691 0.000
+0.433 -4.819 0.000
+0.214 -5.714 0.000
+0.150 -5.604 0.000
+0.088 -5.458 0.000
+0.028 -5.219 0.000
+-0.029 -4.947 0.000
+-0.084 -4.805 0.000
+-0.083 -0.708 0.000
+-0.116 -0.707 0.000
+-0.150 -0.704 0.000
+-0.184 -0.706 0.000
+-0.217 -0.707 0.000
+-0.251 -0.705 0.000
+-0.284 -0.705 0.000
+-0.318 -0.703 0.000
+-0.686 -4.414 0.000
+-0.729 -4.267 0.000
+-0.777 -4.201 0.000
+-0.813 -4.011 0.000
+-0.969 -4.462 0.000
+-1.000 -4.242 0.000
+-1.049 -4.192 0.000
+-1.102 -4.181 0.000
+-1.162 -4.218 0.000
+-1.218 -4.232 0.000
+-1.274 -4.234 0.000
+-1.329 -4.236 0.000
+-1.385 -4.239 0.000
+-1.441 -4.238 0.000
+-1.312 -2.874 0.000
+-1.313 -2.616 0.000
+-1.296 -2.279 0.000
+-1.335 -2.252 0.000
+-1.397 -2.339 0.000
+-1.487 -2.561 0.000
+-1.524 -2.519 0.000
+-1.535 -2.354 0.000
+-1.564 -2.282 0.000
+-1.601 -2.247 0.000
+-1.836 -2.847 0.000
+-1.854 -2.723 0.000
+-1.840 -2.479 0.000
+-2.403 -4.234 0.000
+-2.461 -4.234 0.000
+-2.518 -4.231 0.000
+-2.577 -4.233 0.000
+-2.636 -4.234 0.000
+-2.695 -4.236 0.000
+-2.754 -4.235 0.000
+-2.814 -4.237 0.000
+-2.873 -4.236 0.000
+-2.934 -4.239 0.000
+-2.994 -4.242 0.000
+-3.053 -4.238 0.000
+-3.114 -4.240 0.000
+-3.173 -4.233 0.000
+-3.235 -4.238 0.000
+-3.297 -4.239 0.000
+-3.355 -4.232 0.000
+-3.416 -4.230 0.000
+-3.480 -4.234 0.000
+-3.542 -4.235 0.000
+-3.604 -4.234 0.000
+-3.668 -4.235 0.000
+-3.730 -4.233 0.000
+-3.794 -4.235 0.000
+-3.859 -4.238 0.000
+-3.921 -4.235 0.000
+-6.833 -10.252 0.000
+-6.938 -10.248 0.000
+-7.044 -10.244 0.000
+-7.154 -10.245 0.000
+-7.265 -10.249 0.000
+-7.374 -10.248 0.000
+-7.822 -10.698 0.000
+-7.924 -10.674 0.000
+-5.985 -6.528 0.000
+-6.073 -6.530 0.000
+-6.142 -6.498 0.000
+-6.156 -6.365 0.000
+-5.972 -5.876 0.000
+-6.054 -5.873 0.000
+-3.490 -1.052 0.000
+-3.509 -1.004 0.000
+-3.554 -0.999 0.000
+-3.601 -0.997 0.000
+-3.652 -1.001 0.000
+-3.703 -1.005 0.000
+-3.750 -1.002 0.000
+-3.802 -1.006 0.000
+-3.850 -1.002 0.000
+-3.891 -0.989 0.000
+-7.001 -5.453 0.000
+-7.022 -5.353 0.000
+-7.045 -5.257 0.000
+-7.075 -5.173 0.000
+-7.709 -5.808 0.000
+-8.270 -6.308 0.000
+-8.342 -6.267 0.000
+-1.334 3.201 0.000
+-1.039 3.622 0.000
+-1.059 3.614 0.000
+-1.068 3.620 0.000
+-1.071 3.634 0.000
+-1.074 3.646 0.000
+-1.081 3.654 0.000
+-1.099 3.649 0.000
+-1.116 3.646 0.000
+-1.135 3.640 0.000
+-1.152 3.638 0.000
+-1.170 3.634 0.000
+-1.186 3.632 0.000
+-1.204 3.628 0.000
+-1.220 3.628 0.000
+-1.230 3.634 0.000
+-1.180 3.780 0.000
+-1.186 3.789 0.000
+-1.202 3.788 0.000
+-1.218 3.787 0.000
+-1.231 3.789 0.000
+-1.250 3.785 0.000
+-1.269 3.782 0.000
+-1.284 3.783 0.000
+-1.300 3.783 0.000
+-1.315 3.784 0.000
+-1.328 3.787 0.000
+-1.280 3.845 0.000
+-1.254 3.882 0.000
+-1.267 3.884 0.000
+-1.284 3.884 0.000
+-1.301 3.882 0.000
+-1.320 3.881 0.000
+-1.336 3.881 0.000
+-1.359 3.876 0.000
+-1.379 3.874 0.000
+-1.401 3.870 0.000
+-1.419 3.870 0.000
+-1.440 3.868 0.000
+-1.460 3.867 0.000
+-1.479 3.866 0.000
+-1.499 3.865 0.000
+-1.518 3.866 0.000
+-1.539 3.864 0.000
+-1.560 3.864 0.000
+-1.581 3.864 0.000
+-1.600 3.865 0.000
+-1.618 3.867 0.000
+-1.635 3.869 0.000
+-1.659 3.868 0.000
+-1.684 3.866 0.000
+-1.707 3.865 0.000
+-1.732 3.864 0.000
+-1.753 3.865 0.000
+-1.676 3.930 0.000
+-1.556 4.020 0.000
+-1.539 4.044 0.000
+-1.539 4.057 0.000
+-1.539 4.070 0.000
+-1.538 4.084 0.000
+-1.536 4.098 0.000
+-1.536 4.110 0.000
+-1.536 4.123 0.000
+-1.534 4.136 0.000
+-1.532 4.150 0.000
+-1.532 4.162 0.000
+-1.533 4.174 0.000
+-1.532 4.187 0.000
+-1.531 4.199 0.000
+-1.530 4.212 0.000
+-1.530 4.224 0.000
+-1.527 4.237 0.000
+-1.530 4.247 0.000
+-1.528 4.260 0.000
+-1.527 4.272 0.000
+-1.524 4.285 0.000
+-1.524 4.297 0.000
+-1.523 4.309 0.000
+-1.524 4.320 0.000
+-1.522 4.331 0.000
+-1.523 4.342 0.000
+-1.522 4.354 0.000
+-1.521 4.365 0.000
+-1.520 4.377 0.000
+-1.520 4.388 0.000
+-1.518 4.400 0.000
+-1.519 4.410 0.000
+-1.520 4.420 0.000
+-1.521 4.431 0.000
+-1.528 4.439 0.000
+-1.552 4.441 0.000
+-1.575 4.443 0.000
+-1.601 4.444 0.000
+-1.625 4.447 0.000
+-1.651 4.449 0.000
+-1.679 4.450 0.000
+-1.709 4.452 0.000
+-1.740 4.453 0.000
+-1.771 4.455 0.000
+-1.804 4.456 0.000
+-1.833 4.459 0.000
+-1.866 4.461 0.000
+-1.914 4.459 0.000
+-2.053 4.431 0.000
+-2.598 4.288 0.000
+-2.644 4.292 0.000
+-2.643 4.309 0.000
+-2.651 4.325 0.000
+-2.684 4.333 0.000
+-2.718 4.341 0.000
+-2.761 4.348 0.000
+-2.801 4.356 0.000
+-2.848 4.362 0.000
+-2.894 4.370 0.000
+-2.938 4.378 0.000
+-2.997 4.383 0.000
+-3.112 4.378 0.000
+-3.148 4.390 0.000
+-3.152 4.409 0.000
+-3.158 4.427 0.000
+-3.165 4.445 0.000
+-3.169 4.464 0.000
+-3.176 4.482 0.000
+-3.183 4.501 0.000
+-3.185 4.520 0.000
+-9.609 3.579 0.000
+-9.610 3.636 0.000
+-3.591 4.562 0.000
+-3.587 4.585 0.000
+-3.614 4.603 0.000
+-3.726 4.612 0.000
+-3.899 4.615 0.000
+-3.768 4.652 0.000
+-3.606 4.691 0.000
+-3.578 4.715 0.000
+-3.589 4.736 0.000
+-3.612 4.755 0.000
+-10.173 4.519 0.000
+-10.133 4.580 0.000
+-9.046 4.737 0.000
+-9.126 4.788 0.000
+-9.101 4.842 0.000
+-8.900 4.899 0.000
+-8.828 4.953 0.000
+-8.761 5.005 0.000
+-9.150 5.110 0.000
+-7.099 5.182 0.000
+-3.622 5.165 0.000
+-3.631 5.186 0.000
+-6.796 5.497 0.000
+-6.793 5.537 0.000
+-6.910 5.586 0.000
+-6.939 5.630 0.000
+-6.937 5.671 0.000
+-6.935 5.712 0.000
+-3.938 5.479 0.000
+-3.931 5.502 0.000
+-3.327 5.495 0.000
+-6.601 6.036 0.000
+-6.444 6.051 0.000
+-6.243 6.059 0.000
+-6.069 6.068 0.000
+-5.852 6.067 0.000
+-5.137 6.008 0.000
+-5.127 6.038 0.000
+-5.131 6.070 0.000
+-5.015 6.078 0.000
+-4.881 6.082 0.000
+-4.742 6.082 0.000
+-4.609 6.083 0.000
+-4.477 6.082 0.000
+-3.623 5.935 0.000
+-3.616 5.956 0.000
+-3.617 5.979 0.000
+-3.619 6.003 0.000
+-3.617 6.025 0.000
+-3.618 6.048 0.000
+-3.586 6.085 0.000
+-3.443 6.068 0.000
+-0.201 5.141 0.000
+-0.196 5.142 0.000
+-0.190 5.142 0.000
+-0.189 5.143 0.000
+-0.184 5.143 0.000
+-0.178 5.143 0.000
+-0.177 5.144 0.000
+-0.172 5.144 0.000
+-0.173 5.146 0.000
+-0.172 5.148 0.000
+-0.172 5.149 0.000
+-0.172 5.151 0.000
+-0.173 5.153 0.000
+-0.173 5.154 0.000
+-0.174 5.157 0.000
+-0.177 5.160 0.000
+-0.178 5.162 0.000
+-0.180 5.164 0.000
+-0.181 5.166 0.000
+-0.182 5.169 0.000
+-0.181 5.170 0.000
+-0.177 5.170 0.000
+-0.177 5.172 0.000
+-0.175 5.173 0.000
+-0.176 5.175 0.000
+-0.175 5.177 0.000
+-0.173 5.177 0.000
+-0.180 5.183 0.000
+-0.195 5.192 0.000
+-0.205 5.199 0.000
+-0.222 5.209 0.000
+-0.307 5.255 0.000
+-0.377 5.294 0.000
+-0.492 5.357 0.000
+-1.802 6.072 0.000
+-1.776 6.073 0.000
+-1.756 6.075 0.000
+-1.730 6.075 0.000
+-1.706 6.075 0.000
+-1.684 6.076 0.000
+-1.661 6.076 0.000
+-1.640 6.078 0.000
+-1.619 6.079 0.000
+-1.598 6.080 0.000
+-1.575 6.079 0.000
+-1.553 6.078 0.000
+-1.530 6.077 0.000
+-1.505 6.074 0.000
+-1.476 6.069 0.000
+-0.553 5.480 0.000
+-0.518 5.461 0.000
+-0.500 5.454 0.000
+-0.492 5.454 0.000
+-0.484 5.453 0.000
+-0.479 5.454 0.000
+-0.473 5.455 0.000
+-0.471 5.459 0.000
+-0.472 5.464 0.000
+-0.482 5.476 0.000
+-0.482 5.481 0.000
+-0.488 5.491 0.000
+-0.480 5.490 0.000
+-0.476 5.492 0.000
+-0.476 5.497 0.000
+-0.480 5.506 0.000
+-0.576 5.590 0.000
+-0.693 5.692 0.000
+-0.722 5.723 0.000
+-0.794 5.792 0.000
+-1.061 6.027 0.000
+-1.090 6.063 0.000
+-1.077 6.064 0.000
+-1.064 6.064 0.000
+-1.051 6.064 0.000
+-1.036 6.063 0.000
+-1.023 6.063 0.000
+-1.003 6.056 0.000
+-0.450 5.558 0.000
+-0.230 5.354 0.000
+-0.183 5.312 0.000
+-0.166 5.298 0.000

cloud_2.txt

@@ -0,0 +1,667 @@
+-0.595 -4.305 0.000
+-0.626 -4.293 0.000
+-0.665 -4.291 0.000
+-0.707 -4.290 0.000
+-0.754 -4.293 0.000
+-0.863 -4.353 0.000
+-1.162 -4.582 0.000
+-1.024 -4.415 0.000
+-0.980 -4.334 0.000
+-0.905 -4.227 0.000
+-0.762 -4.063 0.000
+-0.764 -4.026 0.000
+-0.854 -4.064 0.000
+-1.128 -4.255 0.000
+-1.202 -4.276 0.000
+-1.245 -4.270 0.000
+-1.248 -4.232 0.000
+-1.144 -4.108 0.000
+-1.150 -4.073 0.000
+-1.290 -4.144 0.000
+-1.504 -4.268 0.000
+-1.564 -4.272 0.000
+-1.629 -4.279 0.000
+-1.688 -4.281 0.000
+-1.758 -4.290 0.000
+-1.821 -4.293 0.000
+-1.882 -4.294 0.000
+-1.967 -4.312 0.000
+-2.044 -4.322 0.000
+-2.111 -4.324 0.000
+-2.173 -4.323 0.000
+-2.239 -4.324 0.000
+-2.301 -4.321 0.000
+-2.367 -4.320 0.000
+-2.434 -4.319 0.000
+-2.507 -4.320 0.000
+-2.580 -4.321 0.000
+-2.651 -4.320 0.000
+-2.729 -4.322 0.000
+-2.802 -4.321 0.000
+-2.881 -4.322 0.000
+-2.964 -4.325 0.000
+-3.047 -4.327 0.000
+-3.126 -4.325 0.000
+-3.206 -4.323 0.000
+-3.283 -4.319 0.000
+-3.335 -4.300 0.000
+-3.371 -4.271 0.000
+-3.397 -4.237 0.000
+-3.402 -4.192 0.000
+-3.447 -4.168 0.000
+-3.723 -4.263 0.000
+-1.257 -2.901 0.000
+-1.262 -2.873 0.000
+-1.282 -2.851 0.000
+-1.338 -2.847 0.000
+-1.425 -2.857 0.000
+-1.492 -2.856 0.000
+-1.527 -2.840 0.000
+-1.557 -2.822 0.000
+-1.808 -2.768 0.000
+-1.829 -2.744 0.000
+-1.854 -2.721 0.000
+-1.839 -2.650 0.000
+-1.795 -2.602 0.000
+-1.322 -2.396 0.000
+-1.263 -2.346 0.000
+-1.257 -2.316 0.000
+-1.275 -2.294 0.000
+-1.341 -2.289 0.000
+-1.444 -2.295 0.000
+-1.502 -2.286 0.000
+-1.546 -2.271 0.000
+-14.735 -5.628 0.000
+-0.132 -1.237 0.000
+-0.134 -1.218 0.000
+-0.136 -1.200 0.000
+-0.131 -1.181 0.000
+-0.096 -1.159 0.000
+-0.061 -1.136 0.000
+-0.049 -1.117 0.000
+-0.047 -1.099 0.000
+-0.046 -1.081 0.000
+-0.045 -1.063 0.000
+-0.046 -1.045 0.000
+-0.046 -1.027 0.000
+-0.046 -1.010 0.000
+-0.046 -0.992 0.000
+-0.043 -0.974 0.000
+-0.046 -0.956 0.000
+-0.044 -0.938 0.000
+-0.046 -0.921 0.000
+-0.044 -0.903 0.000
+-0.044 -0.885 0.000
+-0.045 -0.867 0.000
+-0.045 -0.850 0.000
+-0.043 -0.832 0.000
+-0.048 -0.814 0.000
+-0.045 -0.796 0.000
+-0.043 -0.779 0.000
+0.022 -0.762 0.000
+0.061 -0.745 0.000
+-0.032 -0.726 0.000
+-13.819 -0.179 0.000
+-9.650 -0.113 0.000
+-9.020 0.416 0.000
+-9.034 0.488 0.000
+-15.484 2.499 0.000
+-9.056 1.935 0.000
+-13.728 3.202 0.000
+-9.663 3.262 0.000
+-9.652 3.340 0.000
+-9.635 3.416 0.000
+-9.619 3.493 0.000
+-9.623 3.576 0.000
+-9.624 3.659 0.000
+-9.633 3.745 0.000
+-9.648 3.834 0.000
+-9.662 3.923 0.000
+-10.135 4.540 0.000
+-10.098 4.614 0.000
+-10.058 4.686 0.000
+-10.033 4.765 0.000
+-10.091 4.880 0.000
+-10.064 4.959 0.000
+-10.079 5.057 0.000
+-10.018 5.121 0.000
+-9.770 5.279 0.000
+-11.176 6.462 0.000
+-10.938 6.443 0.000
+-7.420 5.493 0.000
+-4.204 3.665 0.000
+-4.172 3.703 0.000
+-4.131 3.736 0.000
+-4.111 3.781 0.000
+-4.093 3.828 0.000
+-4.007 3.831 0.000
+-3.943 3.847 0.000
+-3.887 3.867 0.000
+-3.828 3.885 0.000
+-3.766 3.901 0.000
+-3.710 3.920 0.000
+-3.661 3.944 0.000
+-3.667 4.007 0.000
+-3.691 4.084 0.000
+-3.717 4.163 0.000
+-3.744 4.244 0.000
+0.796 0.929 0.000
+0.862 0.897 0.000
+0.889 0.896 0.000
+0.916 0.895 0.000
+0.938 0.897 0.000
+0.958 0.900 0.000
+0.975 0.907 0.000
+0.987 0.916 0.000
+0.994 0.931 0.000
+1.000 0.946 0.000
+1.001 0.966 0.000
+0.998 0.989 0.000
+0.992 1.016 0.000
+0.971 1.056 0.000
+0.920 1.125 0.000
+0.892 1.174 0.000
+0.895 1.195 0.000
+0.901 1.212 0.000
+0.914 1.223 0.000
+0.929 1.232 0.000
+0.943 1.242 0.000
+0.959 1.250 0.000
+0.973 1.259 0.000
+0.983 1.274 0.000
+0.990 1.291 0.000
+0.995 1.310 0.000
+1.001 1.329 0.000
+1.003 1.351 0.000
+-1.488 4.151 0.000
+-1.532 4.257 0.000
+-1.529 4.313 0.000
+-1.525 4.370 0.000
+-2.880 6.078 0.000
+-2.814 6.081 0.000
+-2.746 6.082 0.000
+-2.682 6.086 0.000
+-2.613 6.083 0.000
+-2.547 6.085 0.000
+-2.485 6.089 0.000
+-2.418 6.087 0.000
+-2.351 6.083 0.000
+-2.286 6.083 0.000
+-2.221 6.080 0.000
+-2.161 6.085 0.000
+-2.098 6.083 0.000
+-2.011 6.049 0.000
+-1.944 6.041 0.000
+-1.912 6.080 0.000
+-1.858 6.090 0.000
+-1.798 6.089 0.000
+-1.740 6.091 0.000
+-1.679 6.088 0.000
+-1.623 6.090 0.000
+-1.566 6.092 0.000
+-1.506 6.089 0.000
+-1.447 6.086 0.000
+-1.391 6.087 0.000
+-1.334 6.085 0.000
+-1.277 6.083 0.000
+-1.221 6.083 0.000
+-1.167 6.084 0.000
+-1.113 6.084 0.000
+-1.058 6.084 0.000
+-1.003 6.081 0.000
+-0.947 6.076 0.000
+-0.509 5.483 0.000
+-0.195 5.163 0.000
+-0.258 5.367 0.000
+-0.209 5.360 0.000
+-0.110 5.256 0.000
+-0.057 5.240 0.000
+0.887 3.498 0.000
+0.921 3.491 0.000
+0.953 3.489 0.000
+0.982 3.492 0.000
+1.010 3.498 0.000
+1.032 3.517 0.000
+1.024 3.600 0.000
+0.959 3.818 0.000
+0.989 3.822 0.000
+1.056 3.742 0.000
+2.299 0.892 0.000
+2.313 0.887 0.000
+2.325 0.886 0.000
+2.336 0.885 0.000
+2.348 0.886 0.000
+2.358 0.887 0.000
+2.370 0.885 0.000
+2.381 0.887 0.000
+2.392 0.885 0.000
+2.404 0.884 0.000
+2.414 0.888 0.000
+2.425 0.887 0.000
+2.435 0.889 0.000
+2.445 0.892 0.000
+2.454 0.898 0.000
+2.462 0.909 0.000
+2.468 0.923 0.000
+2.470 0.952 0.000
+2.467 1.001 0.000
+2.466 1.044 0.000
+2.461 1.103 0.000
+2.453 1.176 0.000
+2.463 1.185 0.000
+2.468 1.210 0.000
+2.466 1.267 0.000
+2.471 1.298 0.000
+2.461 1.390 0.000
+1.388 6.089 0.000
+1.431 6.087 0.000
+1.472 6.090 0.000
+1.514 6.089 0.000
+1.556 6.089 0.000
+1.598 6.088 0.000
+1.639 6.092 0.000
+1.681 6.090 0.000
+1.723 6.089 0.000
+2.193 3.857 0.000
+2.281 3.657 0.000
+2.335 3.485 0.000
+2.361 3.479 0.000
+2.386 3.482 0.000
+2.412 3.479 0.000
+2.437 3.478 0.000
+2.462 3.484 0.000
+2.485 3.501 0.000
+2.498 3.611 0.000
+2.506 3.774 0.000
+2.337 6.089 0.000
+2.377 6.089 0.000
+2.418 6.088 0.000
+2.459 6.084 0.000
+2.499 6.086 0.000
+2.539 6.087 0.000
+2.579 6.085 0.000
+2.620 6.084 0.000
+2.660 6.088 0.000
+2.700 6.086 0.000
+2.740 6.086 0.000
+2.781 6.077 0.000
+2.821 6.068 0.000
+2.945 5.556 0.000
+2.983 5.519 0.000
+3.061 6.085 0.000
+3.101 6.085 0.000
+3.141 6.088 0.000
+3.198 5.342 0.000
+3.231 5.269 0.000
+3.269 5.334 0.000
+3.297 5.170 0.000
+3.321 4.978 0.000
+3.350 4.894 0.000
+3.378 4.823 0.000
+3.409 4.795 0.000
+3.442 4.802 0.000
+3.476 4.814 0.000
+3.510 4.828 0.000
+3.545 4.842 0.000
+3.579 4.855 0.000
+3.614 4.868 0.000
+3.649 4.883 0.000
+3.685 4.904 0.000
+3.738 5.053 0.000
+3.800 5.263 0.000
+3.829 5.216 0.000
+3.875 5.283 0.000
+3.916 5.316 0.000
+4.110 6.067 0.000
+4.036 5.164 0.000
+4.054 5.065 0.000
+4.063 4.924 0.000
+4.082 4.844 0.000
+4.134 4.935 0.000
+4.443 6.078 0.000
+4.489 6.098 0.000
+3.888 3.089 0.000
+3.900 3.038 0.000
+3.923 3.034 0.000
+3.949 3.043 0.000
+3.988 3.105 0.000
+4.824 6.078 0.000
+4.867 6.080 0.000
+4.910 6.078 0.000
+4.953 6.077 0.000
+4.996 6.077 0.000
+5.039 6.075 0.000
+5.083 6.077 0.000
+5.128 6.081 0.000
+5.156 6.029 0.000
+4.173 2.767 0.000
+4.189 2.745 0.000
+4.207 2.729 0.000
+4.234 2.743 0.000
+5.530 6.081 0.000
+5.575 6.079 0.000
+5.620 6.079 0.000
+5.666 6.078 0.000
+5.712 6.078 0.000
+5.758 6.076 0.000
+5.804 6.074 0.000
+5.851 6.076 0.000
+5.897 6.072 0.000
+5.945 6.076 0.000
+5.993 6.076 0.000
+6.040 6.075 0.000
+6.088 6.074 0.000
+6.135 6.073 0.000
+6.280 6.070 0.000
+6.329 6.070 0.000
+6.379 6.069 0.000
+6.382 5.975 0.000
+6.411 5.936 0.000
+6.525 6.061 0.000
+6.577 6.065 0.000
+6.516 5.851 0.000
+6.420 5.580 0.000
+6.056 4.744 0.000
+5.997 4.564 0.000
+6.567 5.412 0.000
+6.565 5.327 0.000
+6.563 5.242 0.000
+6.197 4.479 0.000
+6.682 5.124 0.000
+6.723 5.113 0.000
+6.766 5.104 0.000
+6.813 5.103 0.000
+6.860 5.099 0.000
+6.908 5.098 0.000
+6.956 5.095 0.000
+7.004 5.093 0.000
+7.054 5.093 0.000
+7.105 5.092 0.000
+7.156 5.092 0.000
+7.216 5.104 0.000
+7.275 5.114 0.000
+7.322 5.106 0.000
+7.368 5.097 0.000
+7.458 5.146 0.000
+7.493 5.120 0.000
+7.498 5.056 0.000
+7.498 4.986 0.000
+7.498 4.916 0.000
+7.498 4.849 0.000
+7.499 4.783 0.000
+7.497 4.714 0.000
+7.493 4.644 0.000
+7.493 4.580 0.000
+7.495 4.519 0.000
+7.496 4.458 0.000
+6.866 3.615 0.000
+6.794 3.482 0.000
+6.791 3.429 0.000
+6.798 3.387 0.000
+6.823 3.366 0.000
+6.859 3.357 0.000
+6.900 3.352 0.000
+6.940 3.346 0.000
+6.989 3.349 0.000
+7.130 3.446 0.000
+7.189 3.457 0.000
+7.187 3.405 0.000
+5.480 1.632 0.000
+5.413 1.538 0.000
+5.398 1.497 0.000
+5.396 1.468 0.000
+5.401 1.447 0.000
+5.411 1.429 0.000
+5.421 1.413 0.000
+5.433 1.398 0.000
+5.446 1.384 0.000
+5.462 1.373 0.000
+5.479 1.362 0.000
+5.492 1.348 0.000
+4.701 0.648 0.000
+4.597 0.543 0.000
+4.573 0.507 0.000
+4.566 0.486 0.000
+4.561 0.467 0.000
+4.555 0.446 0.000
+4.552 0.429 0.000
+4.544 0.409 0.000
+4.540 0.391 0.000
+4.534 0.372 0.000
+4.527 0.353 0.000
+4.525 0.337 0.000
+4.519 0.319 0.000
+4.512 0.300 0.000
+4.506 0.283 0.000
+4.503 0.267 0.000
+4.498 0.251 0.000
+4.492 0.233 0.000
+4.487 0.217 0.000
+4.483 0.202 0.000
+4.478 0.186 0.000
+4.475 0.171 0.000
+4.471 0.157 0.000
+4.474 0.147 0.000
+4.485 0.141 0.000
+4.496 0.136 0.000
+4.510 0.133 0.000
+4.524 0.129 0.000
+4.538 0.126 0.000
+4.552 0.122 0.000
+4.567 0.118 0.000
+4.583 0.115 0.000
+4.596 0.110 0.000
+4.612 0.107 0.000
+4.625 0.102 0.000
+4.641 0.099 0.000
+4.659 0.096 0.000
+4.672 0.091 0.000
+4.685 0.085 0.000
+4.702 0.081 0.000
+4.719 0.078 0.000
+4.734 0.073 0.000
+4.751 0.068 0.000
+4.768 0.064 0.000
+4.784 0.059 0.000
+4.801 0.055 0.000
+4.818 0.050 0.000
+4.836 0.045 0.000
+4.861 0.044 0.000
+4.928 0.061 0.000
+5.925 0.467 0.000
+5.923 0.446 0.000
+5.925 0.427 0.000
+5.925 0.407 0.000
+5.927 0.388 0.000
+5.929 0.369 0.000
+5.928 0.348 0.000
+5.930 0.330 0.000
+5.931 0.311 0.000
+5.929 0.291 0.000
+5.936 0.273 0.000
+5.936 0.254 0.000
+5.938 0.236 0.000
+5.939 0.217 0.000
+5.941 0.198 0.000
+5.943 0.180 0.000
+5.944 0.161 0.000
+5.940 0.141 0.000
+5.942 0.123 0.000
+5.952 0.107 0.000
+5.953 0.089 0.000
+5.954 0.070 0.000
+5.956 0.052 0.000
+5.959 0.034 0.000
+5.962 0.017 0.000
+5.962 -0.002 0.000
+5.962 -0.020 0.000
+5.969 -0.037 0.000
+5.971 -0.055 0.000
+5.970 -0.073 0.000
+5.974 -0.091 0.000
+5.974 -0.109 0.000
+5.976 -0.127 0.000
+5.977 -0.145 0.000
+5.978 -0.163 0.000
+5.981 -0.180 0.000
+5.979 -0.198 0.000
+5.980 -0.216 0.000
+5.981 -0.234 0.000
+5.985 -0.251 0.000
+5.987 -0.269 0.000
+5.989 -0.286 0.000
+5.990 -0.304 0.000
+5.992 -0.322 0.000
+5.991 -0.340 0.000
+5.994 -0.357 0.000
+5.995 -0.375 0.000
+5.999 -0.392 0.000
+5.998 -0.410 0.000
+6.019 -0.425 0.000
+6.062 -0.437 0.000
+6.054 -0.456 0.000
+6.045 -0.475 0.000
+6.048 -0.493 0.000
+5.286 -0.597 0.000
+5.249 -0.614 0.000
+5.246 -0.627 0.000
+5.243 -0.641 0.000
+3.736 -0.758 0.000
+3.639 -0.768 0.000
+3.626 -0.772 0.000
+3.662 -0.774 0.000
+3.742 -0.775 0.000
+3.897 -0.775 0.000
+5.231 -0.759 0.000
+5.234 -0.771 0.000
+5.233 -0.784 0.000
+5.234 -0.797 0.000
+5.233 -0.810 0.000
+5.231 -0.823 0.000
+5.231 -0.836 0.000
+5.230 -0.849 0.000
+5.229 -0.862 0.000
+5.229 -0.875 0.000
+5.230 -0.888 0.000
+5.231 -0.901 0.000
+5.297 -0.918 0.000
+5.671 -0.952 0.000
+5.885 -0.981 0.000
+5.881 -0.997 0.000
+5.826 -1.010 0.000
+5.618 -1.010 0.000
+5.438 -1.010 0.000
+5.369 -1.018 0.000
+5.341 -1.029 0.000
+5.338 -1.043 0.000
+5.312 -1.054 0.000
+5.225 -1.057 0.000
+5.214 -1.069 0.000
+5.212 -1.082 0.000
+5.213 -1.095 0.000
+5.214 -1.108 0.000
+5.212 -1.121 0.000
+5.213 -1.135 0.000
+5.214 -1.148 0.000
+5.213 -1.161 0.000
+5.214 -1.174 0.000
+5.215 -1.188 0.000
+5.213 -1.201 0.000
+5.214 -1.214 0.000
+5.214 -1.227 0.000
+5.213 -1.241 0.000
+5.214 -1.254 0.000
+5.217 -1.268 0.000
+5.246 -1.288 0.000
+6.680 -1.665 0.000
+8.445 -2.146 0.000
+8.522 -2.200 0.000
+9.004 -2.359 0.000
+8.966 -2.386 0.000
+3.711 -1.000 0.000
+3.637 -0.983 0.000
+3.636 -0.987 0.000
+3.638 -0.992 0.000
+3.637 -0.996 0.000
+3.638 -1.000 0.000
+3.640 -1.005 0.000
+3.642 -1.009 0.000
+3.641 -1.013 0.000
+3.642 -1.018 0.000
+3.640 -1.021 0.000
+3.709 -1.049 0.000
+6.568 -2.115 0.000
+6.485 -2.107 0.000
+6.476 -2.127 0.000
+6.533 -2.172 0.000
+7.971 -2.797 0.000
+7.909 -2.806 0.000
+7.839 -2.810 0.000
+6.850 -2.452 0.000
+6.807 -2.460 0.000
+3.715 -1.124 0.000
+3.651 -1.099 0.000
+3.634 -1.096 0.000
+3.625 -1.096 0.000
+3.616 -1.097 0.000
+3.609 -1.098 0.000
+3.603 -1.099 0.000
+3.596 -1.100 0.000
+3.590 -1.101 0.000
+3.583 -1.102 0.000
+3.576 -1.102 0.000
+3.564 -1.100 0.000
+3.493 -1.066 0.000
+3.440 -1.041 0.000
+3.413 -1.029 0.000
+3.405 -1.028 0.000
+3.398 -1.027 0.000
+3.393 -1.028 0.000
+3.388 -1.028 0.000
+3.384 -1.029 0.000
+3.379 -1.028 0.000
+3.373 -1.028 0.000
+3.367 -1.027 0.000
+3.363 -1.027 0.000
+3.357 -1.027 0.000
+3.352 -1.026 0.000
+3.344 -1.024 0.000
+3.335 -1.021 0.000
+3.326 -1.018 0.000
+3.326 -1.020 0.000
+3.324 -1.022 0.000
+3.323 -1.024 0.000
+3.324 -1.027 0.000
+3.320 -1.027 0.000
+3.316 -1.027 0.000
+3.314 -1.029 0.000
+3.309 -1.028 0.000
+3.307 -1.029 0.000
+3.304 -1.029 0.000
+3.301 -1.030 0.000
+3.297 -1.030 0.000
+3.292 -1.029 0.000
+3.290 -1.029 0.000
+3.287 -1.030 0.000
+3.281 -1.028 0.000
+3.279 -1.029 0.000
+3.276 -1.029 0.000
+3.271 -1.027 0.000
+3.269 -1.029 0.000
+3.266 -1.029 0.000
+3.263 -1.029 0.000
+3.259 -1.028 0.000
+3.256 -1.028 0.000
+3.253 -1.028 0.000
+3.252 -1.030 0.000
+3.249 -1.030 0.000
+3.245 -1.029 0.000
+3.243 -1.030 0.000
+3.240 -1.029 0.000
+3.238 -1.030 0.000
+3.236 -1.030 0.000
+3.234 -1.031 0.000
+3.232 -1.032 0.000
+3.230 -1.033 0.000
+3.228 -1.034 0.000
+3.225 -1.033 0.000

package.xml

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>FindWheel</name>
+  <version>0.0.0</version>
+  <description>The FindWheel package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="mtc@todo.todo">mtc</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/FindWheel</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
+  
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

scripts/EleFence.proto

@@ -0,0 +1,33 @@
+syntax = "proto2";
+package EleFence;
+
+message Region
+{
+    required float minx=1;
+    required float maxx=2;
+    required float miny=3;
+    required float maxy=4;
+}
+
+message Transform2d
+{
+    optional float m00=1 [default=1.0];
+    optional float m01=2 [default=0.0];
+    optional float m02=3 [default=0.0];
+    optional float m10=4 [default=0.0];
+    optional float m11=5 [default=1.0];
+    optional float m12=6 [default=0.0];
+}
+
+message lidarParam
+{
+    optional string topic=1;
+    optional Transform2d transform=2;
+    optional float dist_limit=3 [default=8.0];
+}
+
+message globalParam
+{
+    repeated lidarParam lidar_params=1;
+    repeated Region regions=2;
+}

scripts/setting.prototxt

@@ -0,0 +1,35 @@
+lidar_params
+{
+    topic:"/scan1"
+	transform
+	{
+		m00:0.0132359
+		m01:-0.9999126
+		m02:0.081
+		m10:-0.9999126
+		m11:-0.0132359
+		m12:5.057
+	}
+}
+
+lidar_params
+{
+    topic:"/scan2"
+	transform
+	{
+		m00:0.0134667
+		m01:0.9999093
+		m02:3.002
+		m10:0.9999093
+		m11:-0.0134667
+		m12:-0.803
+	}
+}
+
+regions
+{
+	minx:0
+	maxx:3.3
+	miny:0
+	maxy:5.0
+}

src/FenceController.cpp

@@ -0,0 +1,87 @@
+//
+// Created by zx on 2019/12/6.
+//
+#include "FenceController.h"
+
+bool FenceController::ReadProtoParam(std::string path)
+{
+    int fd = open(path.c_str(), O_RDONLY);
+    if (fd == -1) return false;
+    FileInputStream* input = new FileInputStream(fd);
+    bool success = google::protobuf::TextFormat::Parse(input, &m_global_param);
+    delete input;
+    close(fd);
+    return success;
+}
+
+FenceController::FenceController(std::string path):
+m_initialized(0)
+{
+    p_merged_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    m_initialized = ReadProtoParam(path);
+    const int lidar_size = m_global_param.lidar_params_size();
+    const int region_size = m_global_param.regions_size();
+    p_lidars.resize(lidar_size);
+    p_region_workers.resize(region_size);
+    p_plc_data = PlcData::GetInstance("192.168.0.1");
+    // init lidar instances
+    for (int i = 0; i < m_global_param.lidar_params_size(); ++i) {
+        p_lidars[i] = new Lidar(m_global_param.lidar_params(i), m_handle);
+    }
+    // init region_detector instances
+    for (int j = 0; j < m_global_param.regions_size(); ++j) {
+        p_region_workers[j] = new Region_worker(j, m_global_param.regions(j));
+    }
+    m_cond_exit.Notify(false);
+    m_update_thread = new std::thread(cloudMergeUpdate, this);
+//    while(m_initialized){
+//        cloudMergeUpdate(this);
+//        usleep(1000*50);
+//    }
+}
+
+FenceController::~FenceController()
+{
+    m_cond_exit.Notify(true);
+    if(m_update_thread){
+        if(m_update_thread->joinable()){
+            m_update_thread->join();
+        }
+        delete m_update_thread;
+        m_update_thread = 0;
+    }
+    // delete lidar instances
+    for (int i = 0; i < m_global_param.lidar_params_size(); ++i) {
+        delete p_lidars[i];
+    }
+    // delete region_detector instances
+    for (int j = 0; j < m_global_param.regions_size(); ++j) {
+        delete p_region_workers[j];
+    }
+    PlcData::Release();
+}
+void FenceController::GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out){
+    m_cloud_mutex.lock();
+    cloud_out->clear();
+    cloud_out->operator+=(*p_merged_cloud);
+    m_cloud_mutex.unlock();
+}
+
+void FenceController::cloudMergeUpdate(FenceController* fc)
+{
+    if(fc==0) return;
+    while(!fc->m_cond_exit.WaitFor(1)) {
+        fc->m_cloud_mutex.lock();
+        fc->p_merged_cloud->clear();
+        for (int i = 0; i < fc->p_lidars.size(); ++i) {
+            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+            if (fc->p_lidars[i]->GetCloud(cloud)) {
+                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);
+        }
+        fc->m_cloud_mutex.unlock();
+    }
+}

src/FenceController.h

@@ -0,0 +1,63 @@
+//
+// Created by zx on 2019/12/6.
+//
+#include <iostream>
+#include <string>
+#include <fstream>
+#include <thread>
+#include <unistd.h>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+#include <ros/ros.h>
+
+#include "EleFence.pb.h"
+#include "LogFiles.h"
+#include "Lidar.h"
+#include "region_worker.h"
+#include "unistd.h"
+#include "s7_plc.h"
+#include "PlcData.h"
+
+#ifndef FENCECONTROLLER_H
+#define FENCECONTROLLER_H
+
+
+class FenceController{
+public:
+    FenceController(std::string path);
+    ~FenceController();
+    static void cloudMergeUpdate(FenceController* fc);
+    void GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
+
+private:
+    bool ReadProtoParam(std::string path);
+
+public:
+    bool                                    m_initialized;
+    ros::NodeHandle                         m_handle;
+    std::mutex                              m_cloud_mutex;
+private:
+    EleFence::globalParam                   m_global_param;
+
+    std::vector<Lidar*>                     p_lidars;
+    std::vector<Region_worker*>             p_region_workers;
+
+    PlcData*                                p_plc_data;
+
+    StdCondition                            m_cond_exit;
+    std::thread*                            m_update_thread;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr     p_merged_cloud;
+};
+
+#endif //FENCECONTROLLER_H

src/Lidar.cpp

@@ -0,0 +1,104 @@
+//
+// Created by zx on 2019/12/5.
+//
+
+#include "Lidar.h"
+#include "LogFiles.h"
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr  laserScan2Cloud(const sensor_msgs::LaserScan& msg,
+                                                     Eigen::MatrixXf transform2d, float dist_limit)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    float angle = msg.angle_min;
+    for (size_t i = 0; i < msg.ranges.size(); ++i)
+    {
+        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)
+        {
+            const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
+            Eigen::MatrixXf  mat=rotation * (first_echo * Eigen::Vector3f::UnitX());
+            /*Eigen::Matrix<float,4,1>  e_transform;
+            e_transform<<mat,1.0;
+            Eigen::MatrixXf  new_pos=tf_btol*e_transform;*/
+            float x = mat(0, 0);
+            float y = mat(1, 0);
+            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);
+            point_cloud->points.push_back(point);
+        }
+        angle += msg.angle_increment;
+    }
+    return point_cloud;
+}
+void SaveTxtCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string strFile)
+{
+    CLogFile log;
+    log.open(strFile.c_str());
+    if(log.is_open())
+    {
+        for(int i=0;i<cloud->size();++i)
+        {
+            pcl::PointXYZ point=cloud->points[i];
+            char buf[255]={0};
+            sprintf(buf,"%.3f %.3f %.3f\n",point.x,point.y,point.z);
+            log.write(buf,strlen(buf));
+        }
+        log.close();
+    }
+}
+
+Lidar::Lidar(EleFence::lidarParam lidar_param, ros::NodeHandle handle):
+b_save_cloud(0)
+{
+    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
+    m_lidar_param=lidar_param;
+    m_handle=handle;
+//    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));
+}
+Lidar::~Lidar()
+{
+
+}
+
+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();
+    Eigen::Matrix<float,2,3> transform;
+    transform(0,0)=T.m00();
+    transform(0,1)=T.m01();
+    transform(0,2)=T.m02();
+    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_time = ros::Time::now();
+
+    if(pLidar->b_save_cloud==false)
+    {
+        char buf[255]={0};
+        sprintf(buf,"/home/zx/zzw/catkin_ws/src/FindWheel/cloud_%s.txt",pLidar->m_lidar_param.topic().c_str());
+        SaveTxtCloud(pLidar->m_cloud,buf);
+        std::cout<<" save cloud   "<<buf<<std::endl;
+        pLidar->b_save_cloud=true;
+    }
+}
+
+bool Lidar::GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int timeout_milli)
+{
+    cloud=m_cloud;
+//    std::cout<<cloud->size()<<std::endl;
+    return true;
+    ros::Duration duration = ros::Time::now() - m_cloud_time;
+    if(duration.toSec()*1000 > timeout_milli)
+        return false;
+    else
+        return true;
+}

src/Lidar.h

@@ -0,0 +1,42 @@
+//
+// Created by zx on 2019/12/5.
+//
+
+#ifndef LIDAR_H
+#define LIDAR_H
+#include <ros/ros.h>
+#include "EleFence.pb.h"
+#include <string>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl_ros/transforms.h>
+#include <pcl/filters/passthrough.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+class Lidar
+{
+public:
+    Lidar(EleFence::lidarParam lidar_param, ros::NodeHandle handle);
+    ~Lidar();
+    bool GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int timeout_milli=100);
+
+protected:
+    static void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& msg,Lidar* pLidar);
+
+protected:
+    ros::NodeHandle m_handle;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
+    ros::Time   m_cloud_time;
+
+    ros::Subscriber m_scan_sub;
+    EleFence::lidarParam m_lidar_param;
+
+    bool                b_save_cloud;
+};
+
+
+#endif //LIDAR_H

src/LogFiles.cpp

@@ -0,0 +1,145 @@
+
+#include "LogFiles.h"
+#include <string.h>
+
+const string CLogFiles::m_strFileNameLoglaserBinary1 = "laser1.data";
+const string CLogFiles::m_strFileNameLoglaserValue1 = "laser1.txt";
+const string CLogFiles::m_strFileNameLogPoints1 = "points1.txt";
+
+const string CLogFiles::m_strFileNameLoglaserBinary2 = "laser2.data";
+const string CLogFiles::m_strFileNameLoglaserValue2 = "laser2.txt";
+const string CLogFiles::m_strFileNameLogPoints2 = "points2.txt";
+
+
+
+CLogFile::CLogFile()
+{
+
+}
+
+
+CLogFile::~CLogFile()
+{
+	if (m_file)
+	{
+        m_lock.lock();
+		m_file.close();
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::open(const char *_Filename, bool binary /* = false */)
+{
+	if (!m_file.is_open())
+	{
+        m_lock.lock();
+		if (binary)
+			m_file.open(_Filename, ios_base::out | ios_base::binary);
+		else
+			m_file.open(_Filename, ios_base::out);
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write(const char *_Str,	streamsize _Count)
+{
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+		m_file.write(_Str, _Count);
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write_double(double val)
+{
+	char buffer[512] = { 0 };
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+        sprintf(buffer, "%f ", val);
+		m_file.write(buffer, strlen(buffer));
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write_int(int val)
+{
+	char buffer[512] = { 0 };
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+        sprintf(buffer, "%d ", val);
+		m_file.write(buffer, strlen(buffer));
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::close()
+{
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+		m_file.close();
+        m_lock.unlock();
+	}
+}
+
+/****************************************************************************/
+
+CLogFiles::CLogFiles()
+	: m_bBinary(true)
+	, m_bValue(true)
+	, m_bPoints(true)
+{
+
+}
+
+CLogFiles::~CLogFiles()
+{
+	close_project();
+}
+
+void CLogFiles::close_project()
+{
+	m_logLaserBinary1.close();
+	m_logLaserValue1.close();
+	m_logPoints1.close();
+
+	m_logLaserBinary2.close();
+	m_logLaserValue2.close();
+	m_logPoints2.close();
+}
+
+void CLogFiles::new_project(const char * path)
+{
+	close_project();
+
+	if (m_bBinary) 
+	{
+		string filepath1 = string(path) + m_strFileNameLoglaserBinary1;
+		m_logLaserBinary1.open(filepath1.c_str(), true);
+
+		string filepath3 = string(path) + m_strFileNameLoglaserBinary2;
+		m_logLaserBinary2.open(filepath3.c_str(), true);
+	}
+
+	if (m_bValue)
+	{
+		string filepath1 = string(path) + m_strFileNameLoglaserValue1;
+		m_logLaserValue1.open(filepath1.c_str(), true);
+
+		string filepath3 = string(path) + m_strFileNameLoglaserValue2;
+		m_logLaserValue2.open(filepath3.c_str(), true);
+	}
+
+	if (m_bPoints)
+	{
+		string filepath2 = string(path) + m_strFileNameLogPoints1;
+		m_logPoints1.open(filepath2.c_str(), true);
+
+		string filepath4 = string(path) + m_strFileNameLogPoints2;
+		m_logPoints2.open(filepath4.c_str(), true);
+	}
+	
+}

src/LogFiles.h

@@ -0,0 +1,73 @@
+#ifndef __LOG__FILE__HH
+#define __LOG__FILE__HH
+#include <string>
+#include <fstream>
+#include <mutex>
+
+using namespace std;
+
+class CLogFile
+{
+public:
+	CLogFile();
+	~CLogFile();
+
+	void open(const char *_Filename, bool binary = false);
+	void write(const char *_Str, streamsize _Count);
+	void close();
+	inline bool is_open() const { return m_file.is_open(); };
+
+	void write_double(double val);
+	void write_int(int val);
+
+public:
+	fstream m_file;
+
+public:
+    std::mutex m_lock;
+};
+
+
+class CLogFiles
+{
+public:
+	CLogFiles();
+	~CLogFiles();
+
+	void set_config(bool binary = true, bool value = true, bool points = true)
+	{
+		m_bBinary = binary;
+		m_bValue = value;
+		m_bPoints = points;
+	}
+
+	void new_project(const char * path);
+	void close_project();
+
+private:
+	bool m_bBinary;
+	bool m_bValue;
+	bool m_bPoints;
+
+
+public:		  
+	CLogFile m_logLaserBinary1;
+	CLogFile m_logLaserValue1;
+	CLogFile m_logPoints1;
+		
+	CLogFile m_logLaserBinary2;
+	CLogFile m_logLaserValue2;
+	CLogFile m_logPoints2;
+
+
+public:
+	static const string m_strFileNameLoglaserBinary1;
+	static const string m_strFileNameLoglaserValue1;
+	static const string m_strFileNameLogPoints1;
+
+	static const string m_strFileNameLoglaserBinary2;
+	static const string m_strFileNameLoglaserValue2;
+	static const string m_strFileNameLogPoints2;
+};
+
+#endif

src/PlcData.cpp

@@ -0,0 +1,68 @@
+//
+// Created by zx on 2019/12/9.
+//
+
+#include "PlcData.h"
+#include <string.h>
+
+PlcData* PlcData::g_instance=0;
+
+PlcData* PlcData::GetInstance(std::string ip)
+{
+    if(g_instance==0)
+        g_instance=new PlcData(ip);
+    return g_instance;
+
+}
+void PlcData::UpdateData(short* data,int id)
+{
+    const int length=1;
+    int offset=id*length;
+//    printf("data: %d \n", *data);
+    m_lock.lock();
+    ////判断越界
+    if(offset>=0 && offset+length<=MAX_REGIONS)
+        memcpy(m_data+offset,data,length*sizeof(short));
+    m_lock.unlock();
+}
+
+void PlcData::plcThread(PlcData* p){
+    if(p == 0) return;
+    if(p->p_plc == 0) return;
+    while(!p->m_cond_exit.WaitFor(1)){
+        // 显示雷达结果
+        std::cout<<"result: [";
+        for (int i = 0; i < MAX_REGIONS; ++i) {
+            std::cout<<p->m_data[i]<<" ";
+        }
+        std::cout<<"]"<<std::endl;
+        // 写plc
+        p->p_plc->WriteShorts(ELE_FENCE_DB_NUM, ELE_FENCE_START_ADDR, MAX_REGIONS, p->m_data);
+
+        usleep(p->m_send_interval_milli * 1000);
+    }
+}
+
+PlcData::PlcData(std::string ip):
+p_send_thread(0)
+{
+    memset(m_data, 0, MAX_REGIONS* sizeof(short));
+    p_plc = new S7PLC();
+    if(ip!=""){
+        p_plc->connect(ip);
+    }
+    m_cond_exit.Notify(false);
+    p_send_thread = new std::thread(plcThread, this);
+}
+
+void PlcData::Release(){
+    if(g_instance) {
+        g_instance->m_cond_exit.Notify(true);
+        if(g_instance->p_plc){
+            g_instance->p_plc->disconnect();
+            delete g_instance->p_plc;
+        }
+        delete g_instance;
+        g_instance = 0;
+    }
+}

src/PlcData.h

@@ -0,0 +1,44 @@
+//
+// Created by zx on 2019/12/9.
+//
+
+#ifndef PLCDATA_H
+#define PLCDATA_H
+#include <iostream>
+#include <string.h>
+#include <mutex>
+#include "StdCondition.h"
+#include <thread>
+#include "s7_plc.h"
+#include "unistd.h"
+#define MAX_REGIONS 15
+#define ELE_FENCE_START_ADDR 0
+#define ELE_FENCE_DB_NUM 48
+
+class PlcData
+{
+public:
+    static PlcData* GetInstance(std::string ip="");
+    static void Release();
+    void UpdateData(short* data,int id);
+    static void plcThread(PlcData* p);
+private:
+    static PlcData*                                 g_instance;
+    StdCondition                                    m_cond_exit;
+    std::thread*                                    p_send_thread;
+    S7PLC*                                          p_plc;
+    const int                                       m_send_interval_milli = 200;
+
+
+    PlcData(std::string ip);
+
+protected:
+    std::mutex m_lock;
+    short m_data[MAX_REGIONS];
+
+};
+
+
+
+
+#endif //PLCDATA_H

src/StdCondition.cpp

@@ -0,0 +1,35 @@
+#include "StdCondition.h"
+
+StdCondition::StdCondition():m_value(false)
+{
+}
+
+StdCondition::StdCondition(bool init):m_value(init)
+{
+}
+
+StdCondition::~StdCondition()
+{
+}
+bool StdCondition::isTrue(StdCondition* scd)
+{
+	if (scd == 0)return false;
+	return scd->m_value;
+}
+
+void StdCondition::Wait()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_cv.wait(loc,std::bind(isTrue,this));
+}
+bool StdCondition::WaitFor(unsigned int millisecond)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	return m_cv.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(isTrue, this));
+}
+void StdCondition::Notify(bool istrue)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_value = istrue;
+	m_cv.notify_all();
+}

src/StdCondition.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <thread>  
+#include <mutex>  
+#include <chrono>  
+#include <condition_variable>
+
+class StdCondition
+{
+public:
+	StdCondition();
+	StdCondition(bool init);
+	~StdCondition();
+	void Wait();
+	bool WaitFor(unsigned int millisecond);
+	void Notify(bool istrue);
+
+protected:
+	static bool isTrue(StdCondition* scd);
+protected:
+	bool m_value;
+	std::mutex m_mutex;
+	std::condition_variable m_cv;
+};
+

src/find_wheel_node.cpp

@@ -0,0 +1,120 @@
+#include "FenceController.h"
+
+
+ros::Publisher g_cluster_publisher[4];
+ros::Publisher g_src_publisher;
+
+int main(int argc, char** argv)
+{
+//    EleFence::globalParam param;
+//    if(ReadProtoParam("/home/zx/zzw/catkin_ws/src/FindWheel/scripts/setting.prototxt",param)==false)
+//    {
+//        ROS_ERROR("load proto param failed...");
+//        return -1;
+//    }
+
+    ros::init(argc, argv, "find_wheel");
+    FenceController fc("/home/zx/zzw/catkin_ws/src/FindWheel/scripts/setting.prototxt");
+    if(fc.m_initialized) {
+//
+//        ::ros::NodeHandle g_ros_handle;
+//        int LidarCount = param.lidars_size();
+//        std::vector<Lidar *> pLidars;
+//        pLidars.resize(LidarCount);
+//        for (int i = 0; i < param.lidars_size(); ++i) {
+//
+//        }
+//
+//        for (int i = 0; i < LIDAR_COUNT; ++i) {
+//            g_clouds[i] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
+//
+//        }
+//        for (int i = 0; i < 4; ++i) {
+//            char buf[255] = {0};
+//            sprintf(buf, "/cloud_wheel_%d", i + 1);
+//            g_cluster_publisher[i] = g_ros_handle.advertise<sensor_msgs::PointCloud2>(buf, 1);
+//        }
+//
+//        for (int i = 0; i < LIDAR_COUNT; ++i) {
+//            char topic[255] = {0};
+//            sprintf(topic, "/scan%d", i + 1);
+//            g_cloud_sub[i] =
+//                g_ros_handle.subscribe<sensor_msgs::LaserScan>(topic, 1, boost::bind(cloud_callback, _1, i));
+//
+//        }
+        g_src_publisher = fc.m_handle.advertise<sensor_msgs::PointCloud2>("cloud_in", 1);
+
+/////过滤xy
+
+
+        ros::Rate loop_rate(10);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr merged_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+        while (::ros::ok()) {
+            //g_src_publisher.publish(fc.p_merged_cloud);
+            fc.GetCloud(merged_cloud);
+            if (merged_cloud->size() > 0) {
+                sensor_msgs::PointCloud2 cloudglobalMsg;
+                pcl::toROSMsg(*merged_cloud, cloudglobalMsg);
+                cloudglobalMsg.header.stamp = ros::Time::now();
+                cloudglobalMsg.header.frame_id = "/map";
+                g_src_publisher.publish(cloudglobalMsg);
+            }
+
+//            for (int i = 0; i < fc.m_region_results.size(); ++i) {
+//                std::cout<<fc.m_region_results[i]? "True" : "False";
+//            }
+//            std::cout<<std::endl;
+//            pcl::PointCloud<pcl::PointXYZ> cloud;
+//            for (int i = 0; i < LIDAR_COUNT; ++i)
+//                cloud += (*g_clouds[i]);
+//
+//            if (cloud.size() > 0) {
+//                float minx = g_param.regions(0).minx();
+//                float maxx = g_param.regions(0).maxx();
+//                float miny = g_param.regions(0).miny();
+//                float maxy = g_param.regions(0).maxy();
+//
+//                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+//
+//                pcl::PassThrough<pcl::PointXYZ> pass;
+//                pass.setInputCloud(cloud.makeShared());
+//                pass.setFilterFieldName("x");//设置想在哪个坐标轴上操作
+//                pass.setFilterLimits(minx, maxx);//将x轴的0到1范围内
+//                pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
+//                pass.filter(*cloud_filtered);//输出到结果指针
+//
+//                pass.setInputCloud(cloud_filtered);
+//                pass.setFilterFieldName("y");//设置想在哪个坐标轴上操作
+//                pass.setFilterLimits(miny, maxy);//将x轴的0到1范围内
+//                pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
+//                pass.filter(*cloud_filtered);//输出到结果指针
+//
+//                if (wheel.detect(cloud_filtered)) {
+//                    //发布聚类结果图
+//                    for (int i = 0; i < 4; ++i) {
+//                        if (wheel.m_clouds[i]->size() > 0) {
+//                            sensor_msgs::PointCloud2 cloudglobalMsg;
+//                            pcl::toROSMsg(*wheel.m_clouds[i], cloudglobalMsg);
+//                            cloudglobalMsg.header.stamp = ros::Time::now();
+//                            cloudglobalMsg.header.frame_id = "/map";
+//                            g_cluster_publisher[i].publish(cloudglobalMsg);
+//                        }
+//                    }
+//                }
+//                std::cout << " cloud in size : " << cloud_filtered->size() << std::endl;
+//                if (cloud_filtered->size() > 0) {
+//                    sensor_msgs::PointCloud2 cloudglobalMsg;
+//                    pcl::toROSMsg(*cloud_filtered, cloudglobalMsg);
+//                    cloudglobalMsg.header.stamp = ros::Time::now();
+//                    cloudglobalMsg.header.frame_id = "/map";
+//                    g_src_publisher.publish(cloudglobalMsg);
+//                }
+//            }
+            ros::spinOnce();
+            loop_rate.sleep();
+        }
+    }
+
+    std::cout<<" exit ..."<<std::endl;
+    return 0;
+}

src/region_detect.cpp

@@ -0,0 +1,99 @@
+//
+// Created by zx on 2019/12/6.
+//
+#include "region_detect.h"
+
+Region_detector::Region_detector(int id, EleFence::Region region):m_region_id(-1)
+{
+    m_region_param.CopyFrom(region);
+    m_region_id = id;
+    for (int i = 0; i < 4; ++i) {
+        m_cloud_segs.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    }
+}
+
+Region_detector::~Region_detector()
+{
+
+}
+
+bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
+{
+    if(!m_region_param.has_maxx() || !m_region_param.has_maxy() || !m_region_param.has_minx() || !m_region_param.has_miny())
+        return false;
+//    if(cloud_in->size() <= 0)
+//        return false;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::copyPointCloud(*cloud_in, *cloud_copy);
+    if(cloud_copy->size() <= 0)
+        return false;
+    // 直通滤波, 筛选xy
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PassThrough<pcl::PointXYZ> pass;
+    pass.setInputCloud(cloud_copy);
+    pass.setFilterFieldName("x");//设置想在哪个坐标轴上操作
+    pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx());//将x轴的0到1范围内
+    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
+    pass.filter(*cloud_filtered);//输出到结果指针
+
+    pass.setInputCloud(cloud_filtered);
+    pass.setFilterFieldName("y");//设置想在哪个坐标轴上操作
+    pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy());//将x轴的0到1范围内
+    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
+    pass.filter(*cloud_filtered);//输出到结果指针
+
+    if(cloud_filtered->size() <= 0)
+        return false;
+    // 离群点过滤
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+    sor.setInputCloud(cloud_filtered);
+    sor.setMeanK(4); //K近邻搜索点个数
+    sor.setStddevMulThresh(4.0); //标准差倍数
+    sor.setNegative(false); //保留未滤波点（内点）
+    sor.filter(*cloud_filtered);  //保存滤波结果到cloud_filter
+
+    if(cloud_filtered->size() <= 0)
+        return false;
+
+    /////聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
+    kdtree->setInputCloud(cloud_filtered);
+
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
+    // 设置聚类的最小值 2cm (small values may cause objects to be divided
+    // in several clusters, whereas big values may join objects in a same cluster).
+    clustering.setClusterTolerance(0.2);
+    // 设置聚类的小点数和最大点云数
+    clustering.setMinClusterSize(10);
+    clustering.setMaxClusterSize(500);
+    clustering.setSearchMethod(kdtree);
+    clustering.setInputCloud(cloud_filtered);
+    std::vector<pcl::PointIndices> clusters;
+    clustering.extract(clusters);
+
+    for (int i = 0; i < 4; ++i) {
+        m_cloud_segs[i]->clear();
+    }
+    if(clusters.size()==4)
+    {
+        int j = 0;
+        for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
+        {
+            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
+            //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
+            for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+            {
+                cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
+                cloud_cluster->width = cloud_cluster->points.size();
+                cloud_cluster->height = 1;
+                cloud_cluster->is_dense = true;
+            }
+            m_cloud_segs[j]=cloud_cluster;
+            j++;
+        }
+//        std::cout<<" cluster class:"<<clusters.size()<<std::endl;
+        return true;
+    }
+//    std::cout<< ros::Time::now()<<"    cluster failed   class: "<<clusters.size()<<std::endl;
+    return false;
+}

src/region_detect.h

@@ -0,0 +1,58 @@
+//
+// Created by zx on 2019/12/6.
+//
+
+#ifndef REGION_DETECT_H
+#define REGION_DETECT_H
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_listener.h>
+#include <vector>
+#include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/Dense"
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl_ros/transforms.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#include <boost/thread.hpp>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+#include "EleFence.pb.h"
+#include "StdCondition.h"
+
+class Region_detector{
+public:
+    Region_detector(int id, EleFence::Region region);
+    ~Region_detector();
+    // given region id (start from 0) and cloud, find four wheels
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
+
+
+private:
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr >  m_cloud_segs;
+
+public:
+    EleFence::Region                m_region_param;
+    int                             m_region_id;
+};
+#endif //REGION_DETECT_H

src/region_worker.cpp

@@ -0,0 +1,58 @@
+//
+// Created by zx on 2019/12/9.
+//
+#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>);
+    m_detector = new Region_detector(id, region);
+    m_cond_exit.Notify(false);
+    m_update_thread = new std::thread(detect_loop, this);
+    m_update_thread->detach();
+}
+
+Region_worker::~Region_worker()
+{
+    m_cond_exit.Notify(true);
+    if(m_update_thread){
+        delete m_update_thread;
+    }
+    if(m_detector){
+        delete m_detector;
+    }
+}
+
+void Region_worker::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in)
+{
+    m_mutex.lock();
+    m_cloud->clear();
+    for (int i = 0; i < cloud_in->size(); ++i) {
+        m_cloud->push_back(cloud_in->points[i]);
+    }
+    b_cloud_updated = true;
+    m_mutex.unlock();
+}
+
+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));
+        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;
+        }
+        PlcData* p = PlcData::GetInstance();
+        if(p) {
+            p->UpdateData(p_data, worker->m_detector->m_region_id);
+        }
+        free(p_data);
+        worker->m_mutex.unlock();
+    }
+}

src/region_worker.h

@@ -0,0 +1,31 @@
+//
+// Created by zx on 2019/12/9.
+//
+
+#ifndef REGION_WORKER_H
+#define REGION_WORKER_H
+
+#include "region_detect.h"
+#include "StdCondition.h"
+#include <thread>
+#include <mutex>
+#include <iostream>
+
+class Region_worker{
+public:
+    Region_worker(int id, EleFence::Region region);
+    ~Region_worker();
+    void update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in);
+    static void detect_loop(Region_worker* worker);
+
+private:
+    Region_detector*                                m_detector;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr             m_cloud;
+    bool                                            b_cloud_updated;
+
+    StdCondition                                    m_cond_exit;
+    std::thread*                                    m_update_thread;
+    std::mutex                                      m_mutex;
+};
+
+#endif //REGION_WORKER_H

src/s7_plc.cpp

@@ -0,0 +1,81 @@
+#include "s7_plc.h"
+
+S7PLC::S7PLC():bConnected_(false)
+{
+    
+}
+S7PLC::~S7PLC()
+{
+    disconnect();
+}
+
+
+bool S7PLC::connect(std::string ip)    
+{
+    std::lock_guard<std::mutex> lck (mutex_);
+    int ret=client_.ConnectTo(ip.c_str(),0,1);
+    bConnected_=(ret==0);
+    return bConnected_;
+}
+
+bool S7PLC::ReadShorts(int DBNumber,int start,int size,short* pdata)
+{
+    short* plc_data= (short*)malloc(size*sizeof(short));
+    bool ret=read_dbs(DBNumber,start*sizeof(short),size*sizeof(short),pdata);
+    if(ret)
+    {
+        reverse_byte(pdata,size*sizeof(short),plc_data);
+        for(int i=0;i<size;++i)
+            pdata[i]=plc_data[size-i-1];
+    }
+    free(plc_data);
+    return ret;
+
+}
+    bool S7PLC::WriteShorts(int DBNumber,int start,int size,short* pdata)
+    {
+        short* plc_data=(short*)malloc(size*sizeof(short));
+        reverse_byte(pdata,size*sizeof(short),plc_data);
+        for(int i=0;i<size;++i)
+            pdata[i]=plc_data[size-i-1];
+        free(plc_data);
+        bool ret=write_dbs(DBNumber,start*sizeof(short),size*sizeof(short),pdata);
+        return ret;
+    }
+
+ bool S7PLC::read_dbs(int DBNumber,int start,int size,void* pdata)
+ {
+     std::lock_guard<std::mutex> lck (mutex_);
+     if(bConnected_==false)
+        return false;
+     
+     int ret=client_.AsDBRead(DBNumber,start,size,pdata);
+     return ret == 0;
+ }
+ bool S7PLC::write_dbs(int DBNumber, int start, int size, void *pdata)
+ {
+    
+    std::lock_guard<std::mutex> lck(mutex_);
+    usleep(1000*30);
+     if(bConnected_==false)
+        return false;
+     
+     int ret = client_.AsDBWrite(DBNumber, start, size, pdata);
+
+     return ret == 0;
+ }
+ void S7PLC::disconnect()
+ {
+     std::lock_guard<std::mutex> lck(mutex_);
+     client_.Disconnect();
+ }
+
+ void S7PLC::reverse_byte(void* pdata,int num_byte,void* out)
+ {
+     char* pin=(char*)pdata;
+     char* pout=(char*)out;
+     for(int i=0;i<num_byte;++i)
+     {
+         pout[i]=pin[num_byte-i-1];
+     }
+ }

src/s7_plc.h

@@ -0,0 +1,28 @@
+#ifndef S7__PLC__H
+#define S7__PLC__H
+
+#include <s7_client.h>
+#include <mutex>
+
+class S7PLC
+{
+protected:
+    bool        bConnected_;
+    std::mutex  mutex_;
+    TSnap7Client client_;
+public:
+    S7PLC();
+    ~S7PLC();
+
+    bool connect(std::string ip);
+    bool ReadShorts(int DBNumber,int start,int size,short* pdata);
+    bool WriteShorts(int DBNumber,int start,int size,short* pdata);
+    void disconnect();
+private:
+    bool read_dbs(int DBNumber,int start,int size,void* pdata);
+    bool write_dbs(int DBNumber,int start,int size,void* pdata);
+    void reverse_byte(void* pdata,int num_byte,void* out);
+
+};
+
+#endif // !S7__PLC__H