init

.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

.gitignore

@@ -0,0 +1,34 @@
+
+#Ignore thumbnails created by Windows
+Thumbs.db
+#Ignore files built 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*
+.vs/
+#Nuget packages folder
+packages/
+build/*
+devel/*

src/.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "",
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "includePath": [
+                "/home/youchen/Documents/measure/MainStructure/elecfence_ws/devel/include/**",
+                "/opt/ros/kinetic/include/**",
+                "/home/youchen/Documents/measure/MainStructure/elecfence_ws/src/wj_716_lidar/include/**",
+                "/usr/include/**",
+                "/usr/local/include/snap7"
+            ],
+            "name": "ROS",
+            "intelliSenseMode": "gcc-x64",
+            "compilerPath": "/usr/bin/clang",
+            "cStandard": "c11",
+            "cppStandard": "c++17"
+        }
+    ],
+    "version": 4
+}

src/.vscode/settings.json

@@ -0,0 +1,67 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/youchen/Documents/measure/MainStructure/elecfence_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/kinetic/lib/python2.7/dist-packages"
+    ],
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "strstream": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "cinttypes": "cpp",
+        "codecvt": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "ratio": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "memory": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "numeric": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "utility": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "core": "cpp"
+    }
+}

src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

src/findwheel/.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/*

src/findwheel/.idea/FindWheel.iml

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

src/findwheel/.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>

src/findwheel/.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>

src/findwheel/.idea/vcs.xml

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

src/findwheel/CMakeLists.txt

@@ -0,0 +1,233 @@
+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)
+find_package(OpenCV 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
+/usr/local/include
+${OpenCV_INLCUDE_DIRS}
+./
+)
+
+## 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
+
+link_directories(/usr/local/lib)
+
+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/TaskQueue/BaseTask.cpp 
+    src/TaskQueue/ThreadTaskQueue.cpp 
+    src/TaskQueue/TQFactory.cpp 
+    src/wheelCalcTask.cpp
+    src/globalmsg.pb.cc
+    src/s7_plc.cpp
+        src/PlcData.cpp src/PlcData.h src/define.h)
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} snap7 libnnxx.a libnanomsg.so ${OpenCV_LIBS})
+
+## 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)

src/findwheel/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

src/findwheel/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

src/findwheel/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>

src/findwheel/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;
+}

src/findwheel/scripts/setting.prototxt

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

src/findwheel/src/FenceController.cpp

@@ -0,0 +1,201 @@
+//
+// 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 wheel handling thread and queue
+    m_cond_wheel_exit.Notify(false);
+    m_wheel_calc_queue = tq::TQFactory::CreateDefaultQueue();
+    m_wheel_calc_queue->Start(3);
+    if(m_wheel_soc){
+        m_wheel_soc.bind(CONNECTSTRING);
+    }
+    m_wheel_recv_thread = new std::thread(wheelMsgHandlingThread, this);
+
+    // 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);
+    m_cond_wheel_exit.Notify(true);
+    if(m_update_thread){
+        if(m_update_thread->joinable()){
+            m_update_thread->join();
+        }
+        delete m_update_thread;
+        m_update_thread = 0;
+    }
+    std::cout<<"exit 00"<<std::endl;
+    if(m_wheel_recv_thread){
+        if(m_wheel_recv_thread->joinable()){
+            m_wheel_recv_thread->join();
+        }
+        delete m_wheel_recv_thread;
+        m_wheel_recv_thread = 0;
+    }
+    std::cout<<"exit 01"<<std::endl;
+    if (m_wheel_calc_queue)
+    {
+        m_wheel_calc_queue->CancelAll();
+        m_wheel_calc_queue->Stop();
+        delete m_wheel_calc_queue;
+        m_wheel_calc_queue = 0;
+    }
+    std::cout<<"exit 02"<<std::endl;
+    // 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];
+    }
+    std::cout<<"exit 03"<<std::endl;
+    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);
+            }
+        }
+        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();
+    }
+}
+
+void FenceController::wheelMsgHandlingThread(FenceController* fc){
+    if(fc==0) return;
+    while(!fc->m_cond_wheel_exit.WaitFor(1)){
+        fc->m_mutex_wheel_handling.lock();
+        if(fc->m_wheel_soc){
+            nnxx::message_control* c1 = new nnxx::message_control();
+            auto m1 = fc->m_wheel_soc.recv(1, *c1);
+            if(!m1.empty() && fc->m_wheel_calc_queue){
+                std::cout<<"-------not empty-------"<<std::endl;
+                tq::BaseTask* task = new WheelCalcTask(nnxx::to_string(m1), c1, fc->sendWheelResultCallback, (void *)fc);
+                fc->m_wheel_calc_queue->AddTask(task);
+                // std::cout<<"-------task added-------"<<std::endl;
+            } 
+        }
+        // std::cout<<"++++++++++++++++"<<std::endl;
+        fc->m_mutex_wheel_handling.unlock();
+    }
+}
+
+// termID start from 0
+void FenceController::sendWheelResultCallback(int termID, nnxx::message_control* ctl, void *p){
+    std::cout<<"--------callback start------"<<std::endl;
+    FenceController * fc = ((FenceController *)p);
+    fc->m_mutex_wheel_handling.lock();
+    std::cout<<"--------callback start2------"<<std::endl;
+    for (int i = 0; i < fc->p_region_workers.size(); ++i) {
+        if(termID>=0 && fc->p_region_workers[i]->get_id() == termID){
+            std::cout<<"--------callback find terminal------"<<std::endl;
+            double x=0,y=0,c=0,wheelbase=0, width=0;
+            globalmsg::resultInfo* result = new globalmsg::resultInfo();
+            time_t tt = time(0);
+            struct tm *tc = localtime(&tt);
+            char buf[255] = {0};
+            memset(buf, 0, 255);
+            sprintf(buf, "%d-%02d-%02d %02d:%02d:%02d ", tc->tm_year + 1900,
+                    tc->tm_mon + 1, tc->tm_mday, tc->tm_hour, tc->tm_min, tc->tm_sec);
+            result->set_time(buf);
+            std::cout<<"--------callback get time------"<<std::endl;
+            fc->m_cloud_mutex.lock();
+            pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+            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)) {
+                    total_cloud->operator+=(*cloud);
+                }
+            }
+            fc->m_cloud_mutex.unlock();
+            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)){
+                result->set_correctness(true);
+            }else{
+                result->set_correctness(false);
+            }
+            std::cout<<"--------callback get result------"<<std::endl;
+            result->set_x(x);
+            result->set_y(y);
+            result->set_c(c);
+            result->set_wheel_base(wheelbase);
+            result->set_width(width);
+            std::cout<<"--------callback set result------"<<std::endl;
+
+            // std::cout<<"--------callback serializing------"<<std::endl;
+            std::string result_str = result->SerializeAsString();
+            std::cout<<"--------callback serialized------"<<std::endl;
+            nnxx::message_ostream s;
+            s << result_str;
+            std::cout<<"--------callback handle message------"<<std::endl;
+            nnxx::message m = s.move_msg();
+            
+            if (fc->m_wheel_soc)
+            {
+                fc->m_wheel_soc.send(std::move(m), 0, std::move(*ctl));
+                std::cout<<"------- callback msg sent -------"<<std::endl;
+            }
+            if(ctl){
+                delete ctl;
+            }
+        }
+    }
+    std::cout<<"--------callback end------"<<std::endl;
+    fc->m_mutex_wheel_handling.unlock();
+}

src/findwheel/src/FenceController.h

@@ -0,0 +1,99 @@
+//
+// Created by zx on 2019/12/6.
+//
+#include <iostream>
+#include <string>
+#include <fstream>
+#include <string.h>
+#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 "globalmsg.pb.h"
+
+#include "LogFiles.h"
+#include "Lidar.h"
+#include "region_worker.h"
+#include "unistd.h"
+#include "s7_plc.h"
+#include "PlcData.h"
+
+#include <nnxx/message.h>
+#include <nnxx/message_control.h>
+#include <nnxx/socket.h>
+#include <nnxx/reqrep.h>
+//#include <nnxx/unittest.h>
+#include <nnxx/timeout.h>
+#include <nnxx/error.h>
+#include <cstring>
+#include <nnxx/message>
+
+#include "TaskQueue/TQFactory.h"
+#include "wheelCalcTask.h"
+
+#ifndef FENCECONTROLLER_H
+#define FENCECONTROLLER_H
+
+#define CONNECTSTRING "tcp://127.0.0.1:9000"
+
+class FenceController{
+public:
+    FenceController(std::string path);
+    ~FenceController();
+    // 点云拼接线程
+    static void cloudMergeUpdate(FenceController* fc);
+    // 外部获取点云
+    void GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
+    // 中心轮距指令接收线程
+    static void wheelMsgHandlingThread(FenceController* fc);
+    // 获取轮距回调
+    static void sendWheelResultCallback(int termID, nnxx::message_control* ctl, void *fc);
+
+private:
+    bool ReadProtoParam(std::string path);
+
+public:
+    bool                                    m_initialized;
+    ros::NodeHandle                         m_handle;
+    std::mutex                              m_cloud_mutex;
+    // wheel msg rep
+    nnxx::socket                            m_wheel_soc{nnxx::SP_RAW, nnxx::REP};
+    // wheel handling lock
+    std::mutex                              m_mutex_wheel_handling;
+private:
+    // 全局系统配置
+    EleFence::globalParam                   m_global_param;
+
+    // 雷达与区域句柄
+    std::vector<Lidar*>                     p_lidars;
+    std::vector<Region_worker*>             p_region_workers;
+    // plc 句柄
+    PlcData*                                p_plc_data;
+    // 更新线程
+    StdCondition                            m_cond_exit;
+    std::thread*                            m_update_thread;
+    // 拼接后点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr     p_merged_cloud;
+
+    // 中心轮距相关变量
+    std::thread*                            m_wheel_recv_thread;
+    StdCondition                            m_cond_wheel_exit;
+    // wheel calculate queue
+    tq::IQueue                              *m_wheel_calc_queue;
+
+};
+
+#endif //FENCECONTROLLER_H

src/findwheel/src/Lidar.cpp

@@ -0,0 +1,112 @@
+//
+// 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());
+    std::cout<<log.is_open()<<std::endl;
+    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};
+        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);
+        }
+        else
+        {
+            sprintf(buf, "/home/youchen/data/wj_wheels/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/findwheel/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/findwheel/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/findwheel/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/findwheel/src/PlcData.cpp

@@ -0,0 +1,83 @@
+//
+// 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(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();
+}
+
+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/findwheel/src/PlcData.h

@@ -0,0 +1,46 @@
+//
+// 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"
+#include "define.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(stateCode code,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/findwheel/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/findwheel/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/findwheel/src/TaskQueue/BaseTask.cpp

@@ -0,0 +1,33 @@
+//
+//  BaseTaskQueue.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "BaseTask.h"
+
+namespace tq {
+    BaseTask::BaseTask():ITask(),_cancelled(false)
+    {
+    }
+    
+    void BaseTask::Cancel()
+    {
+        _cancelled = true;
+    }
+    
+    bool BaseTask::IsCancelled() const
+    {
+        return _cancelled;
+    }
+    
+    void BaseTask::Run()
+    {
+        if (_cancelled) {
+            return;
+        }
+        Main();
+    }
+}

src/findwheel/src/TaskQueue/BaseTask.h

@@ -0,0 +1,29 @@
+//
+//  BaseTaskQueue.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__BaseTaskQueue__
+#define __LibDriveRating_CXX__BaseTaskQueue__
+
+#include "TQInterface.h"
+namespace tq
+{
+    class BaseTask : public ITask
+    {
+    private:
+        volatile bool _cancelled;
+    public:
+        BaseTask();
+        void Run();
+        void Cancel();
+        bool IsCancelled() const;
+        
+        virtual void Main() = 0;
+        virtual TaskCategory GetCategory() const {return NoCategory;}
+    };
+}
+#endif /* defined(__LibDriveRating_CXX__BaseTaskQueue__) */

src/findwheel/src/TaskQueue/TQFactory.cpp

@@ -0,0 +1,23 @@
+//
+//  TQFactory.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/10/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "TQFactory.h"
+#include "ThreadTaskQueue.h"
+namespace tq {
+    IQueue* TQFactory::CreateDefaultQueue()
+    {
+        return new ThreadTaskQueue();
+    }
+    
+    void TQFactory::ReleaseQueue(IQueue* queue)
+    {
+        delete queue;
+    }
+    
+    
+}

src/findwheel/src/TaskQueue/TQFactory.h

@@ -0,0 +1,24 @@
+//
+//  TQFactory.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/10/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__TQFactory__
+#define __LibDriveRating_CXX__TQFactory__
+
+#include "TQInterface.h"
+
+namespace tq {
+    class TQFactory
+    {
+    public:
+        static IQueue* CreateDefaultQueue();
+
+        static void ReleaseQueue(IQueue* queue);
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__TQFactory__) */

src/findwheel/src/TaskQueue/TQInterface.h

@@ -0,0 +1,58 @@
+//
+//  TQInterface.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__TQInterface__
+#define __LibDriveRating_CXX__TQInterface__
+
+namespace tq {
+    typedef unsigned long TaskCategory;
+
+    const TaskCategory NoCategory = 0;
+
+    class ITask
+    {
+    public:
+        virtual void Run() = 0;
+        virtual void Cancel() = 0;
+        virtual bool IsCancelled() const = 0;
+        virtual TaskCategory GetCategory() const = 0;
+        virtual ~ITask(){}
+    };
+    
+    typedef void (*TaskRecycler)(ITask* task,void* context);
+
+    class IQueue
+    {
+    public:
+        virtual void Start(unsigned int nThreads = 1) = 0;
+
+        virtual void Stop() = 0;
+
+        virtual void AddTask(ITask* task) = 0;
+        
+        virtual void GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const= 0;
+        
+        virtual unsigned int TaskCount() const = 0;
+        
+        virtual void CancelAll() = 0;
+        
+        virtual void WaitForFinish() = 0;
+
+        virtual void Suspend() = 0;
+        
+        virtual void Resume() = 0;
+        
+        virtual void SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context){}
+
+        virtual void ClearTaskRecycler(TaskCategory cat){}
+
+        virtual ~IQueue() {}
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__TQInterface__) */

src/findwheel/src/TaskQueue/TaskPool.h

@@ -0,0 +1,89 @@
+#ifndef H_TASK_POOL_H
+#define H_TASK_POOL_H
+#include <list>
+#include "threadpp/threadpp.h"
+#include <cstdlib>
+#include <cstdio>
+namespace tq
+{
+    template <typename TaskType>
+    class TaskPool
+    {
+    public:
+        TaskPool(unsigned capacity = 10);
+        ~TaskPool();
+        TaskType* GetTask(TaskType const& taskPrototype);
+        void RecycleTask(TaskType* task);
+        void Purge();
+    private:
+        threadpp::lock _mutex;
+        std::list<TaskType*> _tasks;
+        unsigned _capacity;
+    };
+
+    template <typename TaskType>
+    TaskType* TaskPool<TaskType>::GetTask(TaskType const& taskPrototype)
+    {
+        _mutex.lock();
+        if(!_tasks.empty())
+        {
+
+            TaskType* taskptr = _tasks.front();
+            _tasks.pop_front();
+            _mutex.unlock();
+            new (taskptr)TaskType(taskPrototype);
+            return taskptr;
+        }
+        else
+        {
+            _mutex.unlock();
+            static int newcount = 0;
+            TaskType* taskptr = new TaskType(taskPrototype);
+            return taskptr;
+        }
+    }
+
+    template <typename TaskType>
+    void TaskPool<TaskType>::RecycleTask(TaskType* task)
+    {
+        (*task).~TaskType();
+        _mutex.lock();
+        if(_tasks.size()<_capacity)
+        {
+            _tasks.push_back(task);
+        }
+        else
+        {
+            free(task);
+        }
+        _mutex.unlock();
+    }
+    
+    template <typename TaskType>
+    void TaskPool<TaskType>::Purge()
+    {
+        _mutex.lock();
+        if(!_tasks.empty())
+        {
+            for(typename std::list<TaskType*>::const_iterator it = _tasks.begin();it!=_tasks.end();++it)
+            {
+                free(*it);
+            }
+            _tasks.clear();
+        }
+        _mutex.unlock();
+    }
+
+    template <typename TaskType>
+    TaskPool<TaskType>::TaskPool(unsigned capacity):
+    _capacity(capacity)
+    {
+    }
+
+    template <typename TaskType>
+    TaskPool<TaskType>::~TaskPool()
+    {
+        this->Purge();
+    }
+}
+#endif

src/findwheel/src/TaskQueue/ThreadTaskQueue.cpp

@@ -0,0 +1,286 @@
+//
+//  ThreadTaskQueue.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "ThreadTaskQueue.h"
+#include <algorithm>
+#define WAIT_TIMEOUT 5000
+
+using namespace threadpp;
+
+namespace tq{
+    
+    class QueueRunnable
+    {
+        ThreadTaskQueue* queue;
+        ITask* currentTask;
+    protected:
+        static void run_callback(void*);
+        thread* th;
+        
+        QueueRunnable(ThreadTaskQueue* q):queue(q),currentTask(NULL){}
+        void run();
+        void CancelCurrentTask();
+        bool TaskIsRunning() const;
+        friend class ThreadTaskQueue;
+    };
+    
+    void QueueRunnable::run_callback(void *ctx)
+    {
+        ((QueueRunnable*) ctx)->run();
+    }
+    
+    void QueueRunnable::run()
+    {
+        while (queue->IsStarted()) {
+            queue->LockQueue();
+            ITask* task = queue->NextTask();
+            if (task == NULL) {
+                queue->UnlockQueue();
+                continue;
+            }
+            currentTask = task;
+            queue->UnlockQueue();
+            task->Run();
+            queue->LockQueue();
+            currentTask = NULL;
+            queue->FinishTask(task);
+            queue->NotifyQueue();
+            queue->UnlockQueue();
+        }
+    }
+    
+    void QueueRunnable::CancelCurrentTask()
+    {
+        queue->LockQueue();
+        if(currentTask)
+        {
+            currentTask->Cancel();
+        }
+        queue->UnlockQueue();
+    }
+    
+    bool QueueRunnable::TaskIsRunning() const
+    {
+        return currentTask != NULL;
+    }
+    
+    ThreadTaskQueue::ThreadTaskQueue():_tasklist(),_started(false),_suspended(false)
+    {
+        
+    }
+    
+    void ThreadTaskQueue::Start(unsigned int nThreads)
+    {
+        _mutex.lock();
+        if (_started) {
+            _mutex.unlock();
+            return;
+        }
+        _started = true;
+        _threads.reserve(nThreads);
+        for (int i = 0; i<nThreads; ++i) {
+            QueueRunnable* runnable = new QueueRunnable(this);
+            runnable->th = new thread(QueueRunnable::run_callback, runnable);
+            _threads.push_back(runnable);
+        }
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::Stop()
+    {
+        _mutex.lock();
+        if (!_started) {
+            _mutex.unlock();
+            return;
+        }
+        _started = false;
+        for (std::list<ITask*>::iterator it = _tasklist.begin(); it!= _tasklist.end(); ++it) {
+            delete *it;
+        }
+        _tasklist.clear();
+        _mutex.notify_all();
+        std::vector<QueueRunnable*> copy(_threads);
+        _threads.clear();
+        _mutex.unlock();
+        for (std::vector<QueueRunnable*>::iterator it = copy.begin(); it!=copy.end(); ++it) {
+            (*it)->th->join();
+            thread* t = (*it)->th;
+            delete (*it);
+            delete t;
+        }
+    }
+    
+    bool ThreadTaskQueue::IsStarted() const
+    {
+        return _started;
+    }
+    
+    void ThreadTaskQueue::AddTask(ITask* task)
+    {
+        _mutex.lock();
+        if (_started) {
+            _tasklist.push_back(task);
+            _mutex.notify_all();
+        }
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const
+    {
+        recursivelock* mutex = const_cast<recursivelock*>(&_mutex);
+        mutex->lock();
+        size_t count = 0;
+        for (std::list<ITask*>::const_iterator it = _tasklist.begin(); it!=_tasklist.end(); ++it) {
+            if (count<taskBufSize) {
+                tasksBuf[count] = *it;
+                count++;
+            }
+            else
+            {
+                break;
+            }
+        }
+        mutex->unlock();
+    }
+    
+    unsigned int ThreadTaskQueue::TaskCount() const
+    {
+        recursivelock* mutex = const_cast<recursivelock*>(&_mutex);
+        mutex->lock();
+        unsigned int count = (unsigned int)_tasklist.size();
+        mutex->unlock();
+        return count;
+    }
+    
+    void ThreadTaskQueue::CancelAll()
+    {
+        _mutex.lock();
+        for (std::vector<QueueRunnable*>::iterator it = _threads.begin(); it!=_threads.end(); ++it) {
+            (*it)->CancelCurrentTask();
+        }
+        for (std::list<ITask*>::const_iterator it = _tasklist.begin(); it!=_tasklist.end(); ++it) {
+            (*it)->Cancel();
+        }
+        _mutex.unlock();
+        
+    }
+    
+    void ThreadTaskQueue::WaitForFinish()
+    {
+        
+        while (true) {
+            _mutex.lock();
+            bool isExecuting = false;
+            for (std::vector<QueueRunnable*>::iterator it = _threads.begin(); it!=_threads.end(); ++it) {
+                if ((*it)->TaskIsRunning()) {
+                    isExecuting = true;
+                    break;
+                }
+            }
+            if (!isExecuting&&_tasklist.size() == 0) {
+                _mutex.unlock();
+                break;
+            }
+            _mutex.wait(100);
+            _mutex.unlock();
+        }
+        
+    }
+    
+    void ThreadTaskQueue::Suspend()
+    {
+        _mutex.lock();
+        _suspended = true;
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::Resume()
+    {
+        _mutex.lock();
+        _suspended = false;
+        _mutex.notify_all();
+        _mutex.unlock();
+
+    }
+    
+    void ThreadTaskQueue::NotifyQueue()
+    {
+        _mutex.notify_all();
+    }
+    
+    ITask* ThreadTaskQueue::NextTask()
+    {
+        while (_started && (_tasklist.empty()||_suspended)) {
+            _mutex.wait(WAIT_TIMEOUT);//defensive waiting time limit.
+        }
+        ITask* task = NULL;
+        if (_tasklist.size()>0) {
+            task = _tasklist.front();
+            _tasklist.pop_front();
+        }
+        return task;
+    }
+    
+    inline
+    void ThreadTaskQueue::LockQueue()
+    {
+        _mutex.lock();
+    }
+    
+    inline
+    void ThreadTaskQueue::UnlockQueue()
+    {
+        _mutex.unlock();
+    }
+
+    inline
+    void ThreadTaskQueue::FinishTask(ITask* task)
+    {
+        if(task->GetCategory() != NoCategory)
+        {
+            _recyclerMutex.lock();
+            std::map<TaskCategory,RecyclerPair>::iterator it = _recyclers.find(task->GetCategory());
+            if(it!=_recyclers.end())
+            {
+                RecyclerPair pair = it->second;
+                _recyclerMutex.unlock();
+                pair.recycler(task,pair.context);
+                return;
+            }
+            _recyclerMutex.unlock();
+        }
+        //禁止析构
+        //delete task;
+
+    }
+    
+    void ThreadTaskQueue::SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context)
+    {
+        _recyclerMutex.lock();
+        std::pair<TaskCategory,RecyclerPair> pair(cat, RecyclerPair(recycler,context));
+        _recyclers.insert(pair);
+        _recyclerMutex.unlock();
+    }
+
+    void ThreadTaskQueue::ClearTaskRecycler(TaskCategory cat)
+    {
+        _recyclerMutex.lock();
+        std::map<TaskCategory,RecyclerPair>::iterator it = _recyclers.find(cat);
+        if(it!=_recyclers.end())
+        {
+            _recyclers.erase(it);
+        }
+        _recyclerMutex.unlock();
+    }
+
+    ThreadTaskQueue::~ThreadTaskQueue()
+    {
+        this->Stop();//Defensive stop.
+    }
+    
+}

src/findwheel/src/TaskQueue/ThreadTaskQueue.h

@@ -0,0 +1,89 @@
+//
+//  ThreadTaskQueue.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__ThreadTaskQueue__
+#define __LibDriveRating_CXX__ThreadTaskQueue__
+
+#include "TQInterface.h"
+#include "BaseTask.h"
+#include <list>
+#include <map>
+#include <vector>
+#include "threadpp/threadpp.h"
+
+namespace tq
+{
+    class QueueRunnable;
+    
+    class ThreadTaskQueue:public IQueue
+    {
+    public:
+        ThreadTaskQueue();
+        
+        void Start(unsigned int nThreads = 1);
+        
+        void Stop();
+        
+        bool IsStarted() const;
+        
+        void AddTask(ITask* task);//Will transfer the ownership of task.
+        
+        void GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const;
+        
+        unsigned int TaskCount() const;
+        
+        void CancelAll();
+        
+        void WaitForFinish() ;
+        
+        void Suspend();
+        
+        void Resume();
+
+        void SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context);
+
+        void ClearTaskRecycler(TaskCategory cat);
+        
+        ~ThreadTaskQueue();
+        
+    protected:
+        
+        void LockQueue();
+        
+        void UnlockQueue();
+        
+        void NotifyQueue();
+        
+        ITask* NextTask();
+
+        void FinishTask(ITask* task);
+        
+        friend class QueueRunnable;
+
+    private:
+        struct RecyclerPair
+        {
+            TaskRecycler recycler;
+            void *context;
+            RecyclerPair(TaskRecycler r,void* c):
+                recycler(r),context(c)
+            {
+            }
+        };
+
+        std::map<TaskCategory,RecyclerPair> _recyclers;
+        std::list<ITask*> _tasklist;
+        std::vector<QueueRunnable*> _threads;
+        threadpp::recursivelock _mutex;
+        threadpp::lock _recyclerMutex;
+        bool _started;
+        bool _suspended;
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__ThreadTaskQueue__) */

src/findwheel/src/TaskQueue/threadpp/impl/pthread_lock.h

@@ -0,0 +1,35 @@
+//
+//  pthread_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_lock__
+#define __threadpp__pthread_lock__
+//extern "C"
+//{
+#include <pthread.h>
+//}
+namespace threadpp
+{
+    class pthread_lock
+    {
+        pthread_mutex_t _mutex;
+        pthread_cond_t _cond;
+        void operator=(const pthread_lock& l){};
+        pthread_lock(const pthread_lock& l){};
+    public:
+        pthread_lock();
+        ~pthread_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+#include "pthread_lock.hpp"
+#endif /* defined(__threadpp__pthread_lock__) */

src/findwheel/src/TaskQueue/threadpp/impl/pthread_lock.hpp

@@ -0,0 +1,73 @@
+//
+//  pthread_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__pthread_lock__hpp__
+#define __threadpp__pthread_lock__hpp__
+#include "../threadpp_assert.h"
+#include <errno.h>
+#include <cstring>
+
+#include <sys/time.h>
+
+static inline void timespec_for(struct timespec* t,unsigned long millisecs) {
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    t->tv_nsec = tv.tv_usec*1000 + millisecs*1000000;
+    t->tv_sec = tv.tv_sec + (t->tv_nsec/1000000000);
+    t->tv_nsec = t->tv_nsec%1000000000;
+}
+
+namespace threadpp{
+    inline pthread_lock::pthread_lock()
+    {
+        pthread_mutex_init(&_mutex, NULL);
+        pthread_cond_init(&_cond, NULL);
+    }
+    
+    inline pthread_lock::~pthread_lock()
+    {
+        pthread_mutex_destroy(&_mutex);
+        pthread_cond_destroy(&_cond);
+    }
+    
+    inline void pthread_lock::lock()
+    {
+        int code = pthread_mutex_lock(&_mutex);
+        ASSERT(code == 0, "lock failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::unlock()
+    {
+        int code = pthread_mutex_unlock(&_mutex);
+        ASSERT(code == 0, "unlock failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::wait()
+    {
+        int code = pthread_cond_wait(&_cond, &_mutex);
+        ASSERT(code == 0 || code == ETIMEDOUT, "wait failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::wait(unsigned long millisecs)
+    {
+        struct timespec ts;
+        timespec_for(&ts,millisecs);
+        int code = pthread_cond_timedwait(&_cond, &_mutex, &ts);
+        ASSERT(code == 0 || code == ETIMEDOUT, "timed wait failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::notify()
+    {
+        pthread_cond_signal(&_cond);
+    }
+    
+    inline void pthread_lock::notify_all()
+    {
+        pthread_cond_broadcast(&_cond);
+    }
+}
+#endif

src/findwheel/src/TaskQueue/threadpp/impl/pthread_thread.h

@@ -0,0 +1,45 @@
+//
+//  pthread_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_thread__
+#define __threadpp__pthread_thread__
+#include <pthread.h>
+namespace threadpp
+{
+    class pthread_thread
+    {
+        struct pthread_context
+        {
+            void(*fp)(void* context);
+            void* context;
+        } _context;
+        
+        pthread_t _thread;
+        pthread_thread(){};
+        void operator=(const pthread_thread& t){};
+        pthread_thread(const pthread_thread& t){};
+        static void* pthread_fp_delegate(void*);
+    public:
+        typedef void (*runnable)(void* ctx);
+        typedef unsigned long long id_type;
+        
+        static id_type null_id();
+        
+        pthread_thread(runnable r,void* t);
+        
+        ~pthread_thread();
+        void join();
+        void detach();
+        bool is_equal(const pthread_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+#include "pthread_thread.hpp"
+#endif /* defined(__threadpp__pthread_thread__) */

src/findwheel/src/TaskQueue/threadpp/impl/pthread_thread.hpp

@@ -0,0 +1,110 @@
+//
+//  pthread_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_thread__hpp__
+#define __threadpp__pthread_thread__hpp__
+
+#include <errno.h>
+#include "../threadpp_assert.h"
+#include <cstring>
+#include <cmath>
+#include <unistd.h>
+namespace threadpp
+{
+    inline pthread_thread::id_type pthread_thread::null_id()
+    {
+        return 0;
+    }
+    
+    inline void* pthread_thread::pthread_fp_delegate(void* ctx)
+    {
+        pthread_thread::pthread_context* pctx =static_cast<pthread_thread::pthread_context*>(ctx);
+        pctx->fp(pctx->context);
+        return NULL;
+    }
+    
+    inline pthread_thread::pthread_thread(runnable r,void* t)
+    {
+        _context.fp = r;
+        _context.context = t;
+        int code = pthread_create(&_thread, NULL, pthread_thread::pthread_fp_delegate, &_context);
+        ASSERT(code==0,"create thread failed,error:%s",strerror(code));
+    }
+    
+//    pthread_thread::pthread_thread(runnable r,void* t,float priority)
+//    {
+//        _context.fp = r;
+//        _context.context = t;
+//        pthread_attr_t tattr;
+//        pthread_attr_init(&tattr);
+//        struct sched_param schp;
+//        int policy = SCHED_FIFO;
+//        pthread_attr_getschedpolicy(&tattr, &policy);
+//        pthread_attr_getschedparam(&tattr, &schp);
+//        float pr =fminf(1.0f,fmaxf(0.0f, priority));
+//        schp.sched_priority = sched_get_priority_min(policy) + pr*(sched_get_priority_max(policy) - sched_get_priority_min(policy));
+//        pthread_attr_setschedparam(&tattr, &schp);
+//        int code = pthread_create(&_thread, &tattr, pthread_thread::pthread_fp_delegate, &_context);
+//        ASSERT(code==0,"create thread failed,error:%s",strerror(code));
+//        pthread_attr_destroy(&tattr);
+//    }
+    
+    inline pthread_thread::~pthread_thread()
+    {
+        ASSERT(_thread == 0,"%s","must join or detach a thread before destructing it");
+    }
+    
+    inline void pthread_thread::join()
+    {
+        void* ret = NULL;
+        int code = pthread_join(_thread, &ret);
+        _thread = 0;
+        ASSERT(code==0,"join thread failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_thread::detach()
+    {
+        int code = pthread_detach(_thread);
+        _thread = 0;
+        ASSERT(code==0,"join thread failed,error:%s",strerror(code));
+
+    }
+    
+    inline bool pthread_thread::is_equal(const pthread_thread& t) const
+    {
+        return pthread_equal(_thread, t._thread);
+    }
+    
+    inline void pthread_thread::sleep(unsigned long millisecs)
+    {
+        usleep((useconds_t)millisecs*1000);
+    }
+    
+    inline pthread_thread::id_type pthread_thread::get_id() const
+    {
+#ifdef __APPLE__
+        __uint64_t tid;
+        return pthread_threadid_np(_thread, &tid);
+        return tid;
+#else
+        return (unsigned long long)(_thread);
+#endif
+    }
+    
+    inline pthread_thread::id_type pthread_thread::current_thread_id()
+    {
+#ifdef __APPLE__
+        __uint64_t tid;
+        pthread_threadid_np(pthread_self(), &tid);
+        return tid;
+#else
+        return (unsigned long long)(pthread_self());
+#endif
+    }
+}
+#endif

src/findwheel/src/TaskQueue/threadpp/impl/std_lock.h

@@ -0,0 +1,33 @@
+//
+//  std_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__std_lock__
+#define __threadpp__std_lock__
+#include <mutex>
+#include <condition_variable>
+namespace threadpp
+{
+    class std_lock
+    {
+        std::mutex _mutex;
+        std::condition_variable_any _cond;
+        void operator=(const std_lock& l){};
+        std_lock(const std_lock& l){};
+    public:
+        std_lock();
+        ~std_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+#include "std_lock.hpp"
+#endif /* defined(__threadpp__std_lock__) */

src/findwheel/src/TaskQueue/threadpp/impl/std_lock.hpp

@@ -0,0 +1,52 @@
+//
+//  std_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__std_lock__hpp__
+#define __threadpp__std_lock__hpp__
+#include "../threadpp_assert.h"
+
+namespace threadpp{
+    
+    inline std_lock::std_lock():_mutex(),_cond()
+    {
+    }
+    
+    inline std_lock::~std_lock()
+    {
+    }
+    
+    inline void std_lock::lock()
+    {
+        _mutex.lock();
+    }
+    
+    inline void std_lock::unlock()
+    {
+        _mutex.unlock();
+    }
+    
+    inline void std_lock::wait()
+    {
+        _cond.wait(_mutex);
+    }
+    
+    inline void std_lock::wait(unsigned long millisecs)
+    {
+        _cond.wait_for(_mutex, std::chrono::milliseconds(millisecs));
+    }
+    
+    inline void std_lock::notify()
+    {
+        _cond.notify_one();
+    }
+    
+    inline void std_lock::notify_all()
+    {
+        _cond.notify_all();
+    }
+}
+#endif

src/findwheel/src/TaskQueue/threadpp/impl/std_thread.h

@@ -0,0 +1,40 @@
+//
+//  std_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__std_thread__
+#define __threadpp__std_thread__
+#include <thread>
+
+namespace threadpp
+{
+    class std_thread
+    {
+        std::thread _thread;
+        std_thread(){};
+        void operator=(const std_thread& t){};
+        std_thread(const std_thread& t){};
+    public:
+        typedef unsigned long long id_type;
+        
+        typedef void (*runnable)(void* ctx);
+        
+        static id_type null_id();
+        
+        std_thread(runnable r,void* t);
+        
+        ~std_thread();
+        void join();
+        void detach();
+        bool is_equal(const std_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+#include "std_thread.hpp"
+#endif /* defined(__threadpp__std_thread__) */

src/findwheel/src/TaskQueue/threadpp/impl/std_thread.hpp

@@ -0,0 +1,64 @@
+//
+//  std_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__std_thread__hpp__
+#define __threadpp__std_thread__hpp__
+#include <functional>
+#include <algorithm>
+namespace threadpp
+{
+    inline std_thread::id_type std_thread::null_id()
+    {
+        return 0;
+    }
+    
+    inline void* std_fp_delegate(std_thread::runnable r, void* context)
+    {
+        r(context);
+        return nullptr;
+    }
+    
+    inline std_thread::std_thread(runnable r,void* t):_thread(std::bind(std_fp_delegate,r,t))
+    {
+    }
+    
+    inline std_thread::~std_thread()
+    {
+        
+    }
+    
+    inline void std_thread::join()
+    {
+        _thread.join();
+    }
+    
+    inline void std_thread::detach()
+    {
+        _thread.detach();
+    }
+    
+    inline bool std_thread::is_equal(const std_thread& t) const
+    {
+        return _thread.get_id() == t._thread.get_id();
+    }
+    
+    inline std_thread::id_type std_thread::get_id() const
+    {
+        return std::hash<std::thread::id>()(_thread.get_id());
+    }
+    
+    inline void std_thread::sleep(unsigned long millisecs)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(millisecs));
+    }
+    
+    inline std_thread::id_type std_thread::current_thread_id()
+    {
+        return std::hash<std::thread::id>()(std::this_thread::get_id());
+    }
+}
+#endif

src/findwheel/src/TaskQueue/threadpp/impl/win_lock.h

@@ -0,0 +1,37 @@
+//
+//  win_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__win_lock__
+#define __threadpp__win_lock__
+
+#include <windows.h>
+
+namespace threadpp
+{
+    class win_lock
+    {
+        CRITICAL_SECTION _mutex;
+        CONDITION_VARIABLE _cond;
+        volatile unsigned int _owner;
+        volatile unsigned int _count;
+        void operator=(const win_lock& l){};
+        win_lock(const win_lock& l){};
+    public:
+        win_lock();
+        ~win_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+
+#include "win_lock.hpp"
+#endif /* defined(__threadpp__win_lock__) */

src/findwheel/src/TaskQueue/threadpp/impl/win_lock.hpp

@@ -0,0 +1,80 @@
+//
+//  win_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__win_lock__hpp__
+#define __threadpp__win_lock__hpp__
+
+#include "win_thread.h"
+#include "../threadpp_assert.h"
+#include "stdio.h"
+namespace threadpp{
+    
+    static inline void test_error(const char* title)
+    {
+        DWORD e = GetLastError();
+        ASSERT(e==0,"%s failed,error:%d",title,e);
+    }
+    
+    inline win_lock::win_lock():_owner(0),_count(0)
+    {
+        SetLastError(0);
+        InitializeCriticalSection(&_mutex);
+        InitializeConditionVariable(&_cond);
+        test_error("init");
+    }
+    
+    inline win_lock::~win_lock()
+    {
+        DeleteCriticalSection(&_mutex);
+    }
+    
+    inline void win_lock::lock()
+    {
+        SetLastError(0);
+        
+        EnterCriticalSection(&_mutex);
+        
+        test_error("lock");
+    }
+    
+    inline void win_lock::unlock()
+    {
+        SetLastError(0);
+        LeaveCriticalSection(&_mutex);
+        test_error("unlock");
+    }
+    
+    inline void win_lock::wait()
+    {
+        SetLastError(0);
+        SleepConditionVariableCS(&_cond,&_mutex,0xFFFFFFFF);
+        test_error("wait");
+    }
+    
+    inline void win_lock::wait(unsigned long millisecs)
+    {
+        SetLastError(0);
+        SleepConditionVariableCS(&_cond,&_mutex,millisecs);
+        DWORD e = GetLastError();
+        ASSERT(e==0||e == ERROR_TIMEOUT,"timed wait failed,error:",e);
+    }
+    
+    inline void win_lock::notify()
+    {
+        SetLastError(0);
+        WakeConditionVariable(&_cond);
+        test_error("notify");
+    }
+    
+    inline void win_lock::notify_all()
+    {
+        SetLastError(0);
+        WakeAllConditionVariable(&_cond);
+        test_error("notify all");
+    }
+}
+#endif

src/findwheel/src/TaskQueue/threadpp/impl/win_thread.h

@@ -0,0 +1,53 @@
+//
+//  win_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__win_thread__
+#define __threadpp__win_thread__
+
+#include <windows.h>
+
+namespace threadpp
+{
+    class win_thread
+    {
+        struct win_context
+        {
+            void(*fp)(void*);
+            void* context;
+        } _context;
+#if NO_CRT
+        typedef DWORD handle_t;
+#else
+		typedef unsigned handle_t;
+#endif
+		static handle_t __stdcall win_fp_delegate(void* context);
+        handle_t _thread_id;
+        HANDLE _handle;
+        win_thread(){};
+        void operator=(const win_thread& t){};
+        win_thread(const win_thread& t){};
+    public:
+        typedef void (*runnable)(void* ctx);
+        typedef unsigned int id_type;
+        
+        static id_type null_id();
+        
+        win_thread(runnable r,void* t);
+        
+        ~win_thread();
+        void join();
+        void detach();
+        bool is_equal(const win_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+
+#include "win_thread.hpp"
+#endif /* defined(__threadpp__win_thread__) */

src/findwheel/src/TaskQueue/threadpp/impl/win_thread.hpp

@@ -0,0 +1,81 @@
+//
+//  win_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__win_thread__hpp__
+#define __threadpp__win_thread__hpp__
+
+#include "../threadpp_assert.h"
+#if NO_CRT
+#else
+#include <process.h>
+#endif
+namespace threadpp
+{
+
+    inline win_thread::id_type win_thread::null_id()
+    {
+        return 0;
+    }
+    
+	inline win_thread::handle_t win_thread::win_fp_delegate(void *context)
+    {
+        win_context* wctx = static_cast<win_context*>(context);
+        wctx->fp(wctx->context);
+        return 0;
+    }
+    
+    inline win_thread::win_thread(runnable r,void* t)
+    {
+        _context.fp = r;
+        _context.context = t;
+#if NO_CRT
+		_handle = CreateThread(NULL,NULL,win_thread::win_fp_delegate,&_context,0,&_thread_id);
+#else
+		_handle = (HANDLE)_beginthreadex(NULL,0,win_thread::win_fp_delegate,&_context,0,&_thread_id);
+#endif		
+    }
+    
+    inline win_thread::~win_thread()
+    {
+        ASSERT(_handle == 0,"%s","must join or detach a thread before destructing it");
+    }
+    
+    inline void win_thread::join()
+    {
+        unsigned code = WaitForSingleObject(_handle,0xFFFFFFFF);
+        CloseHandle(_handle);
+        _handle = 0;
+        ASSERT(code == WAIT_OBJECT_0 || code == WAIT_ABANDONED, "failed to join,error:%d",code);
+    }
+    
+    inline void win_thread::detach()
+    {
+		CloseHandle(_handle);
+        _handle = 0;
+    }
+    
+    inline bool win_thread::is_equal(const win_thread& t) const
+    {
+        return _thread_id == t._thread_id;
+    }
+    
+    inline void win_thread::sleep(unsigned long millisecs)
+    {
+		Sleep(millisecs);
+    }
+    
+	inline win_thread::id_type win_thread::get_id() const
+	{
+		return static_cast<unsigned int>(_thread_id);
+	}
+
+	inline win_thread::id_type win_thread::current_thread_id()
+	{
+		return  static_cast<unsigned int>(GetCurrentThreadId());
+	}
+}
+#endif

src/findwheel/src/TaskQueue/threadpp/recursive_lock.h

@@ -0,0 +1,101 @@
+//
+//  recursive_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/16/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_recursive_lock_h
+#define threadpp_recursive_lock_h
+#include "threadpp_assert.h"
+namespace threadpp
+{
+    template <typename locktype,typename threadtype>
+    class recursive_lock
+    {
+        locktype _lock;
+        volatile typename threadtype::id_type _owner;
+        volatile unsigned int _count;
+    public:
+        recursive_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+    
+    template <typename locktype,typename threadtype>
+    recursive_lock<locktype,threadtype>::recursive_lock():_owner(threadtype::null_id()),_count(0)
+    {
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::lock()
+    {
+        typename threadtype::id_type tid = threadtype::current_thread_id();
+        if(tid == _owner)
+        {
+            _count++;
+        }
+        else
+        {
+            _lock.lock();
+            _owner = tid;
+            _count=1;
+        }
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::unlock()
+    {
+        typename threadtype::id_type tid = threadtype::current_thread_id();
+        ASSERT(tid == _owner,"%s", "unlock failed,try to unlock not owned mutex");
+        _count--;
+        if (_count == 0) {
+            _owner = threadtype::null_id();
+            _lock.unlock();
+        }
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::wait()
+    {
+        typename threadtype::id_type owner = _owner;
+        unsigned count = _count;
+        _owner = threadtype::null_id();
+        _count = 0;
+        _lock.wait();
+        _owner = owner;
+        _count = count;
+
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::wait(unsigned long millisecs)
+    {
+        typename threadtype::id_type owner = _owner;
+        unsigned count = _count;
+        _owner = threadtype::null_id();
+        _count = 0;
+        _lock.wait(millisecs);
+        _owner = owner;
+        _count = count;
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::notify()
+    {
+        _lock.notify();
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::notify_all()
+    {
+        _lock.notify_all();
+    }
+}
+
+#endif

src/findwheel/src/TaskQueue/threadpp/threadpp.h

@@ -0,0 +1,40 @@
+//
+//  threadpp.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_threadpp_h
+#define threadpp_threadpp_h
+#include "recursive_lock.h"
+#if __posix || __APPLE__ || __linux
+#include "impl/pthread_thread.h"
+#include "impl/pthread_lock.h"
+namespace threadpp
+{
+    typedef pthread_thread thread;
+    typedef pthread_lock lock;
+    typedef recursive_lock<pthread_lock, pthread_thread> recursivelock;
+}
+#elif defined(WIN32)
+#include "impl/win_thread.h"
+#include "impl/win_lock.h"
+namespace threadpp
+{
+    typedef win_thread thread;
+    typedef win_lock lock;
+    typedef recursive_lock<win_lock, win_thread> recursivelock;
+}
+#elif __cplusplus>=201103L
+#include "impl/std_thread.h"
+#include "impl/std_lock.h"
+namespace threadpp
+{
+    typedef std_thread thread;
+    typedef std_lock lock;
+    typedef recursive_lock<std_lock, std_thread> recursivelock;
+}
+#endif
+#endif

src/findwheel/src/TaskQueue/threadpp/threadpp_assert.h

@@ -0,0 +1,33 @@
+//
+//  defines.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_assert_h
+#define threadpp_assert_h
+
+//forward VC++ DEBUG symbol.
+#if defined(_DEBUG) && !defined(DEBUG)
+#define DEBUG _DEBUG
+#endif
+
+#if DEBUG //assertions
+#ifdef __cplusplus
+#include <cassert>
+#include <cstdio>
+#else
+#include <assert.h>
+#include <stdio.h>
+#endif
+#define ASSERT0(__cond__) assert(__cond__)
+#define ASSERT(__cond__,__msg__,...) \
+do {if(!(__cond__)){printf(__msg__,__VA_ARGS__);printf("\n");assert(false);}}while(0)
+#else
+#define ASSERT0(__cond__)
+#define ASSERT(__cond__,__msg__,...)
+#endif //assertions
+
+#endif

src/findwheel/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/findwheel/src/find_wheel_node.cpp

@@ -0,0 +1,121 @@
+#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");
+    FenceController fc("/home/youchen/Documents/measure/MainStructure/elecfence_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/findwheel/src/region_detect.cpp

@@ -0,0 +1,184 @@
+//
+// 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>));
+    }
+    cut_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+}
+
+Region_detector::~Region_detector()
+{
+
+}
+
+bool Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out){
+    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;
+    cloud_out->clear();
+    pcl::copyPointCloud(*cloud_in, *cloud_out);
+    if(cloud_out->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_out);
+    pass.setFilterFieldName("x");//设置想在哪个坐标轴上操作
+    pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx());//将x轴的0到1范围内
+    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
+    pass.filter(*cloud_out);//输出到结果指针
+
+    pass.setInputCloud(cloud_out);
+    pass.setFilterFieldName("y");//设置想在哪个坐标轴上操作
+    pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy());//将x轴的0到1范围内
+    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
+    pass.filter(*cloud_out);//输出到结果指针
+
+    if(cloud_out->size() <= 0)
+        return false;
+    // 离群点过滤
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+    sor.setInputCloud(cloud_out);
+    sor.setMeanK(4); //K近邻搜索点个数
+    sor.setStddevMulThresh(4.0); //标准差倍数
+    sor.setNegative(false); //保留未滤波点（内点）
+    sor.filter(*cloud_out);  //保存滤波结果到cloud_filter
+
+    if(cloud_out->size() <= 0)
+        return false;
+    else
+        return true;
+}
+
+bool Region_detector::findWheelbase()
+{
+    if (cut_cloud->size() <= 0)
+        return false;
+    
+    return true;
+}
+
+bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in){
+    /////聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
+    kdtree->setInputCloud(cut_cloud);
+
+    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(cut_cloud);
+    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(cut_cloud->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;
+    }
+    return false;
+}
+
+bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
+{
+    bool result = false;
+    preprocess(cloud_in, cut_cloud);
+
+    if (cut_cloud->size() <= 0)
+        return false;
+    m_seg_mutex.lock();
+    result = clustering(cut_cloud);
+    m_seg_mutex.unlock();
+    return result;
+}
+
+bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, double &x, double &y, double &c, double &wheelbase, double &width)
+{
+    bool result = false;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filterd(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    preprocess(cloud_in, cloud_filterd);
+    // std::cout<<"--------detector after preprocess------"<<std::endl;
+
+    if (cloud_filterd->size() <= 0)
+        return false;
+
+    m_seg_mutex.lock();
+    result = clustering(cloud_filterd);
+    // std::cout<<"--------detector after clustering------"<<std::endl;
+    if(result){
+        // 获得四类，计算中心点角度等
+        std::vector<Eigen::Vector4f> centers;
+        std::vector<cv::Point2f> center_2d_points;
+        Eigen::Vector4f car_center;
+        for (size_t i = 0; i < 4; i++)
+        {
+            // 创建存储点云重心的对象
+            Eigen::Vector4f centroid;
+            if(0 == pcl::compute3DCentroid(*m_cloud_segs[i], centroid))
+                return false;
+            centers.push_back(centroid);
+            car_center += centroid;
+            center_2d_points.push_back(cv::Point2f(centroid[0], centroid[1]));
+        }
+        // std::cout<<"--------detector find center------"<<std::endl;
+        car_center /= 4.0;
+        x = car_center[0];
+        y = car_center[1];
+
+        cv::RotatedRect wheel_box = cv::minAreaRect(center_2d_points);
+        // std::cout<<"--------detector find rect -----"<<std::endl;
+        cv::Point2f vec;
+        cv::Point2f vertice[4];
+        wheel_box.points(vertice);
+        float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
+        float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
+        if (len1 > len2)
+        {
+            vec.x = vertice[0].x - vertice[1].x;
+            vec.y = vertice[0].y - vertice[1].y;
+        }
+        else
+        {
+            vec.x = vertice[1].x - vertice[2].x;
+            vec.y = vertice[1].y - vertice[2].y;
+        }
+        float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+        c = angle_x;
+        wheelbase = std::max(wheel_box.size.width, wheel_box.size.height);
+        width = std::min(wheel_box.size.width, wheel_box.size.height);
+        std::cout<<"--------detector find all------"<<std::endl;
+    }
+
+    m_seg_mutex.unlock();
+    return result;
+}

src/findwheel/src/region_detect.h

@@ -0,0 +1,71 @@
+//
+// Created by zx on 2019/12/6.
+//
+
+#ifndef REGION_DETECT_H
+#define REGION_DETECT_H
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <mutex>
+#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 <pcl/common/centroid.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"
+#include "opencv2/opencv.hpp"
+
+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);
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, double &x, double &y, double &c, double &wheelbase, double &width);
+
+private:
+    bool preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
+    bool clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
+    bool findWheelbase();
+
+private:
+    std::mutex              m_seg_mutex;
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_cloud_segs;
+    // 剪切后点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cut_cloud;
+
+public:
+    EleFence::Region m_region_param;
+    int m_region_id;
+};
+
+#endif //REGION_DETECT_H

src/findwheel/src/region_worker.cpp

@@ -0,0 +1,78 @@
+//
+// 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){
+        if(m_update_thread->joinable())
+            m_update_thread->join();
+        delete m_update_thread;
+    }
+    if(m_detector){
+        delete m_detector;
+    }
+}
+
+int Region_worker::get_id(){
+    if(m_detector){
+        return m_detector->m_region_id;
+    }else{
+        return -1;
+    }
+}
+
+bool Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width){
+    if(m_detector){
+        // std::cout<<"worker getting result"<<std::endl;
+        return m_detector->detect(cloud_in, x, y, c, wheelbase, width);
+    }else{
+        return false;
+    }
+}
+
+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)){
+        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;
+            if(result){
+                code = stateCode ::eOk;
+            }else{
+                code = stateCode ::eOutOfPlace;
+            }
+        }
+        PlcData* p = PlcData::GetInstance();
+        if(p) {
+            p->UpdateData(code, worker->m_detector->m_region_id);
+        }
+        worker->m_mutex.unlock();
+    }
+}

src/findwheel/src/region_worker.h

@@ -0,0 +1,34 @@
+//
+// 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>
+#include "define.h"
+
+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);
+    int get_id();
+    bool get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width);
+
+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/findwheel/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/findwheel/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

src/findwheel/src/wheelCalcTask.cpp

@@ -0,0 +1,31 @@
+#include "wheelCalcTask.h"
+
+WheelCalcTask::WheelCalcTask(std::string termStr, nnxx::message_control *ctl, msgCallback func, void* p){
+    // std::cout<<"task contructor start"<<std::endl;
+    m_msg_callback_func = func;
+    m_base_string = termStr;
+    m_wind = p;
+    // std::cout<<"task contructor 2"<<std::endl;
+    m_ctl=ctl;
+    // std::cout<<"task contructor end"<<std::endl;
+}
+
+WheelCalcTask::~WheelCalcTask(){
+    if(m_msg_callback_func)
+        m_msg_callback_func=NULL;
+}
+
+void WheelCalcTask::Main(){
+    if(m_msg_callback_func){
+        // std::cout<<"recv: "<<m_base_string<<std::endl;
+        if(m_base_string.size()>8 && m_base_string.substr(0, 8) == "Terminal"){
+            int termID = atoi(m_base_string.substr(8, m_base_string.length()-8).c_str());
+            std::cout<<"terminal id: "<<termID<<std::endl;
+            m_msg_callback_func(termID, m_ctl, m_wind);
+        }
+    }
+}
+
+void WheelCalcTask::Cancel(){
+
+}

src/findwheel/src/wheelCalcTask.h

@@ -0,0 +1,35 @@
+#ifndef WHEEL_CALC_TASK_H
+#define WHEEL_CALC_TASK_H
+
+#include <string.h>
+#include <string>
+#include <iostream>
+#include <mutex>
+#include <thread>
+#include "TaskQueue/BaseTask.h"
+#include "globalmsg.pb.h"
+
+#include <nnxx/message.h>
+#include <nnxx/message_control.h>
+#include <nnxx/socket.h>
+#include <nnxx/reqrep.h>
+//#include <nnxx/unittest.h>
+#include <nnxx/timeout.h>
+#include <nnxx/error.h>
+
+typedef void(*msgCallback)(int termID,nnxx::message_control *ctl, void *p);
+
+class WheelCalcTask : public tq::BaseTask{
+public:
+    WheelCalcTask(std::string termStr, nnxx::message_control *ctl, msgCallback func, void* p);
+    ~WheelCalcTask();
+    virtual void   Main();
+    virtual void   Cancel();
+private:
+    std::string     m_base_string;
+    msgCallback     m_msg_callback_func;
+    void*           m_wind;
+    nnxx::message_control* m_ctl;
+};
+
+#endif // WHEEL_CALC_TASK_H

src/wj_716_lidar/CMakeLists.txt

@@ -0,0 +1,205 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(wj_716_lidar)
+
+## 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(catkin REQUIRED COMPONENTS
+  roscpp
+  dynamic_reconfigure
+
+)
+
+## 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/wj_716_lidar.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 wj_716_lidar
+#  CATKIN_DEPENDS roscpp
+#  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}
+)
+
+add_executable(wj_716_lidar src/wj_716_lidar_01.cpp src/async_client.cpp src/wj_716_lidar_protocol.cpp )
+add_dependencies(wj_716_lidar ${PROJECT_NAME}_gencfg)
+target_link_libraries(wj_716_lidar ${catkin_LIBRARIES})
+
+#add_dependencies(wj_716_lidar ${PROJECT_NAME}_gencfg)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/wj_716_lidar.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/wj_716_lidar_node.cpp)
+
+## 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_wj_716_lidar.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)

src/wj_716_lidar/cfg/wj_716_lidar.cfg

@@ -0,0 +1,30 @@
+#! /usr/bin/env python
+
+PACKAGE='wj_716_lidar'
+from dynamic_reconfigure.parameter_generator_catkin import *
+#from math import pi
+
+#from driver_base.msg import SensorLevels
+
+gen = ParameterGenerator()
+#       Name              Type      Reconfiguration level             Description                                      Default    Min       Max
+gen.add("min_ang",        double_t, 0, "The angle of the first range measurement [rad].",                               -3.14,    -3.14,    3.14)
+gen.add("max_ang",        double_t, 0, "The angle of the last range measurement [rad].",                                3.14,     -3.14,    3.14)
+gen.add("angle_increment",double_t, 0, "The angle_increment of the first range measurement [rad].",                     0.00582,  0.00582,  0.00582)
+gen.add("time_increment", double_t, 0, "The time_increment[s].",                                                        0.00006167129,   0.00006167129,   0.00006167129)
+gen.add("range_min",      int_t,    0, "The range_min [m].",                                                            0,        0,        30)
+gen.add("range_max",      int_t,    0, "The range_max[m].",                                                             30,       0,        30)
+gen.add("resize",         int_t,    0, "The resize[num].",                                                              811,      0,        811)
+gen.add("frame_id",       str_t,    0, "The TF frame in which laser scans will be returned.",                        "laser")
+#gen.add("intensity",      bool_t,   SensorLevels.RECONFIGURE_RUNNING, "Whether or not the TiM3xx returns intensity values.",                        True)
+# gen.add("cluster",        int_t,    SensorLevels.RECONFIGURE_RUNNING, "The number of adjacent range measurements to cluster into a single reading.", 1,         0,   99)
+#gen.add("skip",           int_t,    SensorLevels.RECONFIGURE_RUNNING, "The number of scans to skip between each measured scan.",                    0,         0,    9)
+# gen.add("port",           str_t,    SensorLevels.RECONFIGURE_CLOSE,   "The serial port where the TiM3xx device can be found.",                       "/dev/ttyACM0")
+# gen.add("calibrate_time", bool_t,   SensorLevels.RECONFIGURE_CLOSE,   "Whether the node should calibrate the TiM3xx's time offset.",                 True)
+
+#gen.add("time_offset",    double_t, SensorLevels.RECONFIGURE_RUNNING, "An offset to add to the time stamp before publication of a scan [s].",       -0.001,     -0.25, 0.25)
+
+exit(gen.generate(PACKAGE, "wj_716_lidar", "wj_716_lidar"))
+
+
+

src/wj_716_lidar/launch/wj_716_lidar_01.launch

@@ -0,0 +1,143 @@
+<?xml version="1.0"?>
+<!-- start and stop angle is given in [rad] -->
+<!--
+default min_angle is -135 degree.
+default max_angle is +135 degree.
+
+Check IP-address, if you scanner is not found after roslaunch.
+-->
+
+<launch>
+  <node name="wj_716_lidar_01" pkg="wj_716_lidar" type="wj_716_lidar" respawn="false" output="screen">
+  <param name="hostname"         type="string"  value="192.168.2.204" />
+  <param name="port"             type="string"  value="2110" />
+  <!-- -135° -->
+  <param name="min_ang"          type="double"  value="-2.35619449" />
+  <!-- 135° -->
+  <param name="max_ang"          type="double"  value="2.35619449" />
+  <param name="angle_increment"  type="double"  value="0.00582" />
+  <param name="frame_id"         type="str"     value="laser" />
+  <param name="time_increment"   type="double"  value="0.00006167129" />
+  <param name="range_min"        type="double"  value="0.05" />
+  <param name="range_max"        type="double"  value="30.0" />
+  <param name="resize"           type="int"     value="811" />
+
+  </node>
+</launch>
+
+<!--
+Conversion between degree and rad
+
+DEG	RAD
+-180	-3.141592654
+-175	-3.054326191
+-170	-2.967059728
+-165	-2.879793266
+-160	-2.792526803
+-155	-2.705260341
+-150	-2.617993878
+-145	-2.530727415
+-140	-2.443460953
+-137.5	-2.3998277
+-135	-2.35619449
+-130	-2.268928028
+-125	-2.181661565
+-120	-2.094395102
+-115	-2.00712864
+-110	-1.919862177
+-105	-1.832595715
+-100	-1.745329252
+-95	-1.658062789
+-90	-1.570796327
+-85	-1.483529864
+-80	-1.396263402
+-75	-1.308996939
+-70	-1.221730476
+-65	-1.134464014
+-60	-1.047197551
+-55	-0.959931089
+-50	-0.872664626
+-45	-0.785398163
+-40	-0.698131701
+-35	-0.610865238
+-30	-0.523598776
+-25	-0.436332313
+-20	-0.34906585
+-15	-0.261799388
+-10	-0.174532925
+-5	-0.087266463
+0	0
+5	0.087266463
+10	0.174532925
+15	0.261799388
+20	0.34906585
+25	0.436332313
+30	0.523598776
+35	0.610865238
+40	0.698131701
+45	0.785398163
+50	0.872664626
+55	0.959931089
+60	1.047197551
+65	1.134464014
+70	1.221730476
+75	1.308996939
+80	1.396263402
+85	1.483529864
+90	1.570796327
+95	1.658062789
+100	1.745329252
+105	1.832595715
+110	1.919862177
+115	2.00712864
+120	2.094395102
+125	2.181661565
+130	2.268928028
+135	2.35619449
+137.5	2,3998277
+140	2.443460953
+145	2.530727415
+150	2.617993878
+155	2.705260341
+160	2.792526803
+165	2.879793266
+170	2.967059728
+175	3.054326191
+180	3.141592654
+185	3.228859116
+190	3.316125579
+195	3.403392041
+200	3.490658504
+205	3.577924967
+210	3.665191429
+215	3.752457892
+220	3.839724354
+225	3.926990817
+230	4.01425728
+235	4.101523742
+240	4.188790205
+245	4.276056667
+250	4.36332313
+255	4.450589593
+260	4.537856055
+265	4.625122518
+270	4.71238898
+275	4.799655443
+280	4.886921906
+285	4.974188368
+290	5.061454831
+295	5.148721293
+300	5.235987756
+305	5.323254219
+310	5.410520681
+315	5.497787144
+320	5.585053606
+325	5.672320069
+330	5.759586532
+335	5.846852994
+340	5.934119457
+345	6.021385919
+350	6.108652382
+355	6.195918845
+360	6.283185307
+-->

src/wj_716_lidar/package.xml

@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>wj_716_lidar</name>
+  <version>0.1.0</version>
+  <description>The wj_716_lidar package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zsy@todo.todo">zsy</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>Apache 2.0</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/wj_716_lidar</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> -->
+  <author >zsy</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_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+  <build_depend>dynamic_reconfigure</build_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/wj_716_lidar/src/async_client.cpp

@@ -0,0 +1,78 @@
+#include "async_client.h"
+
+Async_Client::Async_Client(boost::asio::io_service& io_service,ip::tcp::endpoint endpoint,fundata_t fundata_ )
+  :iosev(io_service),
+    socket(iosev),
+    ep(endpoint),
+    m_connected(0),
+    m_fundata(fundata_)
+{
+  socket.connect(ep,ec);
+  if(ec)
+  {
+    std::cout << boost::system::system_error(ec).what() << std::endl;
+    m_connected = 0 ;
+  }
+  else
+  {
+    cout<<" Connection Success! "<<ep.address().to_string()<<endl;
+    m_connected = 1 ;
+  }
+  client_async_read();
+}
+void Async_Client::client_async_write(char *buf,int len)
+{
+  boost::asio::async_write(socket,
+                           boost::asio::buffer(buf, len),
+                           boost::bind(&Async_Client::handle_write, this,
+                                       boost::asio::placeholders::error));
+}
+void Async_Client::client_async_read()
+{
+  socket.async_read_some(boost::asio::buffer(data_, MAX_LENGTH),
+                         boost::bind(&Async_Client::handle_read, this,
+                                     boost::asio::placeholders::error,
+                                     boost::asio::placeholders::bytes_transferred));
+}
+bool Async_Client::client_return_status(){
+  return m_connected ;
+}
+
+
+void Async_Client::handle_read(const boost::system::error_code& error,
+                               size_t bytes_transferred)
+{
+  if (!error)
+  {
+    //printf("From %s Received data Len:%d\n",this->ep.address().to_string().c_str(),
+          // bytes_transferred);
+    (*m_fundata)(this->ep.address().to_string().c_str(),this->ep.port(),data_,bytes_transferred);
+    this->client_async_read();
+  }
+  else
+  {
+    printf("%s\n",error.message().c_str());
+    m_connected = 0 ;
+  }
+}
+
+void Async_Client::handle_write(const boost::system::error_code& error)
+{
+  if (!error)
+  {
+
+  }
+  else
+  {
+    printf("%s\n",error.message().c_str());
+    m_connected = 0 ;
+  }
+}
+
+void Async_Client::socket_close()
+{
+
+  socket.close();
+
+}
+

src/wj_716_lidar/src/async_client.h

@@ -0,0 +1,47 @@
+#ifndef ASYNC_CLIENT_H
+#define ASYNC_CLIENT_H
+#include <iostream>
+#include <stdio.h>
+#include <time.h>
+#include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
+#include <string>
+#include <unistd.h>
+
+using namespace std ;
+using boost::asio::ip::udp;
+using boost::asio::ip::tcp;
+using namespace boost::asio;
+#define MAX_LENGTH 50000
+typedef void (*fundata_t)(const char* addr,int port,const char* data,const int len);
+class Async_Client
+{
+public:
+  Async_Client();
+  Async_Client(boost::asio::io_service& io_service,ip::tcp::endpoint endpoint,fundata_t fundata_ );
+  void client_async_write(char *buf,int len);
+  void client_async_read();
+  bool client_return_status();
+
+  void socket_close(void);
+
+private:
+  void handle_read(const boost::system::error_code& error,
+                   size_t bytes_transferred);
+
+  void handle_write(const boost::system::error_code& error);
+
+
+
+  io_service &iosev;
+  ip::tcp::socket socket ;
+  ip::tcp::endpoint ep ;
+  boost::system::error_code ec;
+
+  char        data_[MAX_LENGTH];
+  bool        m_connected ;
+  fundata_t   m_fundata ;
+};
+
+#endif // ASYNC_CLIENT_H

src/wj_716_lidar/src/wj_716_lidar_01.cpp

@@ -0,0 +1,145 @@
+#include <ros/ros.h>
+#include "async_client.h"
+#include "wj_716_lidar_protocol.h"
+using namespace wj_lidar;
+
+int InitTcpConnection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
+{
+
+  io_service iosev;
+
+  ip::tcp::endpoint ep(ip::address_v4::from_string(addr),port);
+  *client_ = new Async_Client(iosev,ep,fundata_);
+
+  iosev.run();
+
+  return 1 ;
+}
+
+int boost_tcp_init_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
+{
+  int timecnt=0 ;
+  *client_ = NULL ;
+
+  boost::thread tmp(&InitTcpConnection,addr,port,client_,fundata_);
+  tmp.detach() ;
+
+  while(timecnt<50){
+    timecnt++ ;
+    usleep(20000); //20 ms
+    if((*client_)->client_return_status()){
+      return 0 ;
+    }
+  }
+  *client_ = NULL ;
+  return -1 ;
+
+}
+
+int boost_tcp_sync_send(Async_Client *client_ ,const char* msg,const int len)
+{
+  if(client_==NULL || client_->client_return_status()==0 ){
+    printf("not connected , please connect first \n");
+    return -1 ;
+  }
+  else{
+    client_->client_async_write((char*)msg,len);
+    return 0 ;
+  }
+
+  return 1;
+}
+
+int boost_tcp_sync_read(Async_Client *client_ )
+{
+  if(client_==NULL || client_->client_return_status()==0 ){
+    printf("not connected , please connect first \n");
+    return -1 ;
+  }
+  else{
+    client_->client_async_read();
+    return 0 ;
+  }
+  return 1;
+}
+
+/* ------------------------------------------------------------------------------------------
+ *  show demo --
+ * ------------------------------------------------------------------------------------------ */
+wj_716_lidar_protocol *protocol;
+Async_Client *client;
+void CallBackRead(const char* addr,int port,const char* data,const int len)
+{
+  protocol->dataProcess(data,len);
+}
+
+void callback(wj_716_lidar::wj_716_lidarConfig &config,uint32_t level)
+{
+  protocol->setConfig(config,level);
+}
+
+void timerCallback(const ros::TimerEvent&)
+{
+//  char heartframe[34]={0xFF,0xAA,0x00,0x1E,0x00,0x00,0x00,0x00,
+//                       0x00,0x00,0x01,0x01,0x00,0x05,0x00,0x00,
+//                       0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x04,
+//                       0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1A,
+//                       0xEE,0xEE};
+
+//  //cout << "20ms" <<endl;
+//  boost_tcp_sync_send(client,heartframe,34);
+
+  if(protocol->heartstate)
+  {
+    protocol->heartstate=false;
+    cout <<"once heart cycle "<<endl;
+  }
+  else
+  {
+    client->socket_close();
+    client=NULL;
+    ros::NodeHandle nh("~");
+    std::string hostname;
+    nh.getParam("hostname",hostname);
+    std::string port;
+    nh.getParam("port",port);
+
+
+    //InitTcpConnection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+   //boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+    boost_tcp_sync_read(client);
+
+    cout <<"try to reconect "<<endl;
+    cout << hostname.c_str()<<endl;
+    cout << port.c_str()<<endl;
+    //ROS_INFO("Hello wj_716_lidar!");
+  }
+
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "wj_716_lidar_01");
+  ros::NodeHandle nh("~");
+//  ros::Timer timer;
+  std::string hostname;
+  nh.getParam("hostname",hostname);
+  cout << hostname <<endl;
+  std::string port;
+  nh.getParam("port",port);
+
+  protocol = new wj_716_lidar_protocol();
+  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig> server;
+  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig>::CallbackType f;
+  f = boost::bind(&callback,_1,_2);
+  server.setCallback(f);
+  client=NULL;
+
+  boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+//  timer= nh.createTimer(ros::Duration(2), timerCallback);
+  boost_tcp_sync_read(client);
+
+  ROS_INFO("Hello wj_716_lidar!");
+  ros::spin();
+
+}

src/wj_716_lidar/src/wj_716_lidar_protocol.cpp

@@ -0,0 +1,371 @@
+#include "wj_716_lidar_protocol.h"
+#include <iostream>
+
+namespace wj_lidar
+{
+  bool wj_716_lidar_protocol::setConfig(wj_716_lidar::wj_716_lidarConfig &new_config,uint32_t level)
+  {
+    config_ = new_config;
+    scan.header.frame_id = config_.frame_id;
+    scan.angle_min = config_.min_ang;
+    scan.angle_max = config_.max_ang;
+    scan.angle_increment = config_.angle_increment;
+    scan.time_increment = config_.time_increment;//0.00002469;
+    scan.range_min = config_.range_min;
+    scan.range_max = config_.range_max;
+//    scan.ranges.resize(config_.resize);
+    scan.intensities.resize(config_.resize);
+
+    scan_TwoEcho.header.frame_id = config_.frame_id;
+    scan_TwoEcho.angle_min = config_.min_ang;
+    scan_TwoEcho.angle_max = config_.max_ang;
+    scan_TwoEcho.angle_increment = config_.angle_increment;
+    scan_TwoEcho.time_increment = config_.time_increment;//0.00002469;
+    scan_TwoEcho.range_min = config_.range_min;
+    scan_TwoEcho.range_max = config_.range_max;
+//    scan.ranges.resize(config_.resize);
+    scan.intensities.resize(config_.resize);
+
+    cout << "frame_id:" <<scan.header.frame_id<<endl;
+    cout << "min_ang:" <<scan.angle_min<<endl;
+    cout << "max_ang:" <<scan.angle_max<<endl;
+    cout << "angle_increment:" <<scan.angle_increment<<endl;
+    cout << "time_increment:" <<scan.time_increment<<endl;
+    cout << "range_min:" <<scan.range_min<<endl;
+    cout << "range_max:" <<scan.range_max<<endl;
+    int resizeNum;
+    cout << "resizeNum:" <<resizeNum<<endl;
+    return true;
+  }
+   wj_716_lidar_protocol::wj_716_lidar_protocol()
+   {
+     memset(&m_sdata,0,sizeof(m_sdata));
+     marker_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 50);
+     twoecho_=nh.advertise<sensor_msgs::LaserScan>("scan_TwoEcho", 5);
+     ros::Time scan_time = ros::Time::now();      //make a virtual data per sec
+     scan.header.stamp = scan_time;
+
+     g_u32PreFrameNo = 0;
+
+     scan.header.frame_id = "wj_716_lidar_frame";
+     scan.angle_min = -2.35619449;
+     scan.angle_max = 2.35619449;
+     scan.angle_increment = 0.00582;
+     scan.time_increment = 0.0667/1081;
+     scan.range_min = 0;
+     scan.range_max = 30;
+     scan.ranges.resize(811);
+     scan.intensities.resize(811);
+
+     scan_TwoEcho.header.frame_id = "wj_716_lidar_frame";
+     scan_TwoEcho.angle_min = -2.35619449;
+     scan_TwoEcho.angle_max = 2.35619449;
+     scan_TwoEcho.angle_increment = 0.00582;
+     scan_TwoEcho.time_increment = 0.0667/1081;
+     scan_TwoEcho.range_min = 0;
+     scan_TwoEcho.range_max = 50;
+     scan_TwoEcho.ranges.resize(811);
+     scan_TwoEcho.intensities.resize(811);
+     cout << "wj_716_lidar_protocl start success" << endl;
+
+   }
+   bool wj_716_lidar_protocol::dataProcess(const char *data, const int reclen)
+   {
+     if(reclen > MAX_LENGTH_DATA_PROCESS)
+     {
+       return false;
+     }
+
+     if(m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS)
+     {
+       memset(&m_sdata,0,sizeof(m_sdata));
+       return false;
+     }
+     memcpy(&m_sdata.m_acdata[m_sdata.m_u32in],data,reclen*sizeof(char));
+     m_sdata.m_u32in += reclen;
+     while(m_sdata.m_u32out < m_sdata.m_u32in)
+     {
+       if(m_sdata.m_acdata[m_sdata.m_u32out] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out+1] == 0x02 &&
+          m_sdata.m_acdata[m_sdata.m_u32out+2] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out+3] == 0x02)
+       {
+         unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 4] << 24) |
+                                 (m_sdata.m_acdata[m_sdata.m_u32out + 5] << 16) |
+                                 (m_sdata.m_acdata[m_sdata.m_u32out + 6] << 8)  |
+                                 (m_sdata.m_acdata[m_sdata.m_u32out + 7] << 0);
+         l_u32reallen = l_u32reallen + 9;
+
+         if(l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1))
+         {
+           if(OnRecvProcess(&m_sdata.m_acdata[m_sdata.m_u32out],l_u32reallen))
+           {
+             m_sdata.m_u32out += l_u32reallen;
+           }
+           else
+           {
+             cout << "continuous frame"<<endl;
+             int i;
+             for(i == 1; i<l_u32reallen; i++)
+             {
+               if((m_sdata.m_acdata[m_sdata.m_u32out + i] == 0x02) &&
+                  (m_sdata.m_acdata[m_sdata.m_u32out + i + 1] == 0x02) &&
+                  (m_sdata.m_acdata[m_sdata.m_u32out + i + 2] == 0x02) &&
+                  (m_sdata.m_acdata[m_sdata.m_u32out + i + 3] == 0x02))
+               {
+                 m_sdata.m_u32out += i;
+                 break;
+               }
+               if(i == l_u32reallen)
+               {
+                 m_sdata.m_u32out += l_u32reallen;
+               }
+             }
+           }
+         }
+         else if(l_u32reallen >= MAX_LENGTH_DATA_PROCESS)
+         {
+           cout << "l_u32reallen >= MAX_LENGTH_DATA_PROCESS"<<endl;
+           cout << "reallen: "<<l_u32reallen<<endl;
+           memset(&m_sdata,0,sizeof(m_sdata));
+
+         }
+         else
+         {
+           //cout<<"reallen: "<<l_u32reallen<<" indata: "<<m_sdata.m_u32in<<" outdata: "<<m_sdata.m_u32out<<endl;
+           break;
+         }
+       }
+       else
+       {
+         m_sdata.m_u32out++;
+       }
+     } //end while(m_sdata.m_u32out < m_sdata.m_u32in)
+
+     if(m_sdata.m_u32out >= m_sdata.m_u32in)
+     {
+       memset(&m_sdata,0,sizeof(m_sdata));
+     }
+      return true;
+   }
+
+   bool wj_716_lidar_protocol::OnRecvProcess( char *data, int len)
+   {
+     if(len > 0)
+     {
+       if(checkXor(data,len))
+       {
+         protocl(data,len);
+       }
+     }
+     else
+     {
+       return false;
+     }
+     return true;
+   }
+
+   bool wj_716_lidar_protocol::protocl(const char *data, const int len)
+   {
+     if((data[8] == 0x73 && data[9] == 0x52)||(data[8] == 0x73 && data[9] == 0x53) )   //command type:0x73 0x52/0X53
+     {
+       static int s_n32ScanIndex;
+       int l_n32PackageNo= (data[50] << 8) + data[51];                                        //shuju bao xu hao
+       unsigned int l_u32FrameNo = (data[46]<<24) + (data[47]<<16) + (data[48]<<8) + data[49];        //quan hao
+
+       if(l_n32PackageNo == 1)
+       {
+         s_n32ScanIndex = 0;
+         g_u32PreFrameNo = l_u32FrameNo;
+
+         for(int j = 0; j < 810;j=j+2)
+         {
+           scandata[s_n32ScanIndex] = (((unsigned char)data[85+j]) << 8) + ((unsigned char)data[86+j]);
+           scandata[s_n32ScanIndex] /= 1000.0;
+           if(scandata[s_n32ScanIndex]>=25 || scandata[s_n32ScanIndex]==0)
+           {
+             scandata[s_n32ScanIndex]= NAN;
+           }
+           s_n32ScanIndex++;
+         }
+          //cout << "quan hao1: " << g_u32PreFrameNo << "  danzhen quanhao1: " << l_u32FrameNo<< endl;
+       }
+       else if(l_n32PackageNo == 2)
+       {
+         if(g_u32PreFrameNo == l_u32FrameNo)
+         {
+           for(int j = 0; j < 812;j=j+2)
+           {
+             scandata[s_n32ScanIndex] =(((unsigned char)data[85+j]) << 8) + ((unsigned char)data[86+j]);
+             scandata[s_n32ScanIndex] /= 1000.0;
+             if(scandata[s_n32ScanIndex]>=25 || scandata[s_n32ScanIndex]==0)
+             {
+               scandata[s_n32ScanIndex]= NAN;
+             }
+             scan.intensities[s_n32ScanIndex] = 0;
+             s_n32ScanIndex++;
+           }
+
+         }
+         else
+         {
+           s_n32ScanIndex = 0;
+           //cout << "quan hao2: " << g_u32PreFrameNo << "  danzhen quanhao2: " << l_u32FrameNo<< endl;
+           return false;
+         }
+       }
+       else if(l_n32PackageNo == 3)
+       {
+         s_n32ScanIndex=0;
+         if(g_u32PreFrameNo == l_u32FrameNo)
+         {
+           for(int j = 0; j < 810;j=j+2)
+           {
+             scaninden[s_n32ScanIndex] =(((unsigned char)data[85+j]) << 8) + ((unsigned char)data[86+j]);
+             s_n32ScanIndex++;
+           }
+         }
+         else
+         {
+           s_n32ScanIndex = 0;
+           //cout << "quan hao3: " << g_u32PreFrameNo << "  danzhen quanhao3: " << l_u32FrameNo<< endl;
+           return false;
+         }
+       }
+       else if(l_n32PackageNo == 4)
+       {
+         if(g_u32PreFrameNo == l_u32FrameNo)
+         {
+           for(int j = 0; j < 812;j=j+2)
+           {
+             scaninden[s_n32ScanIndex] =(((unsigned char)data[85+j]) << 8) + ((unsigned char)data[86+j]);
+             s_n32ScanIndex++;
+           }
+
+           // adjust angle_min to min_ang config param
+           int index_min = (config_.min_ang+2.35619449)/scan.angle_increment;
+           // adjust angle_max to max_ang config param
+           int index_max =  811-((2.35619449-config_.max_ang)/ scan.angle_increment);
+           scan.ranges.resize(index_max-index_min);
+           scan.intensities.resize(index_max-index_min);
+
+           for (int j = index_min; j <= index_max; ++j)
+           {
+               scan.ranges[j - index_min] = scandata[j];
+               scan.intensities[j - index_min]=scaninden[j];
+           }
+         }
+         else
+         {
+           s_n32ScanIndex = 0;
+           //cout << "quan hao4: " << g_u32PreFrameNo << "  danzhen quanhao4: " << l_u32FrameNo<< endl;
+           return false;
+         }
+
+
+       }
+       else if(l_n32PackageNo == 5)
+       {
+         s_n32ScanIndex=0;
+         if(g_u32PreFrameNo == l_u32FrameNo)
+         {
+           for(int j = 0; j < 810;j=j+2)
+           {
+             scandata_te[s_n32ScanIndex] =(((unsigned char)data[85+j]) << 8) + ((unsigned char)data[86+j]);
+             scandata_te[s_n32ScanIndex] /= 1000.0;
+             if(scandata_te[s_n32ScanIndex]>=55 || scandata_te[s_n32ScanIndex]==0)
+             {
+               scandata_te[s_n32ScanIndex]= NAN;
+             }
+             s_n32ScanIndex++;
+           }
+         }
+         else
+         {
+           s_n32ScanIndex = 0;
+           //cout << "quan hao5: " << g_u32PreFrameNo << "  danzhen quanhao5: " << l_u32FrameNo<< endl;
+           return false;
+         }
+       }
+       else if(l_n32PackageNo == 6)
+       {
+         if(g_u32PreFrameNo == l_u32FrameNo)
+         {
+           for(int j = 0; j < 812;j=j+2)
+           {
+             scandata_te[s_n32ScanIndex] =(((unsigned char)data[85+j]) << 8) + ((unsigned char)data[86+j]);
+             scandata_te[s_n32ScanIndex] /= 1000.0;
+             if(scandata_te[s_n32ScanIndex]>=55 || scandata_te[s_n32ScanIndex]==0)
+             {
+               scandata_te[s_n32ScanIndex]= NAN;
+             }
+             s_n32ScanIndex++;
+           }
+
+           int index_min = (config_.min_ang+2.35619449)/scan_TwoEcho.angle_increment;
+           // adjust angle_max to max_ang config param
+           int index_max =  811-((2.35619449-config_.max_ang)/ scan_TwoEcho.angle_increment);
+           scan_TwoEcho.ranges.resize(index_max-index_min);
+           scan_TwoEcho.intensities.resize(index_max-index_min);
+
+           for (int j = index_min; j <= index_max; ++j)
+           {
+               scan_TwoEcho.ranges[j - index_min] = scandata_te[j];
+               scan_TwoEcho.intensities[j - index_min] =0;
+           }
+
+           ros::Time scan_time = ros::Time::now();
+           scan.header.stamp = scan_time;
+           marker_pub.publish(scan);
+
+           ros::Time scan_time1 = ros::Time::now();      //make a virtual data per sec
+           scan_TwoEcho.header.stamp = scan_time1;
+           twoecho_.publish(scan_TwoEcho);
+           //cout<<"time_increament_ "<<scan.time_increment<<endl;
+
+         }
+         else
+         {
+           s_n32ScanIndex = 0;
+           //cout << "quan hao6: " << g_u32PreFrameNo << "  danzhen quanhao6: " << l_u32FrameNo<< endl;
+           return false;
+         }
+
+
+
+       }
+       return true;
+     }
+     else
+     {
+       return false;
+     }
+
+   }
+
+   bool wj_716_lidar_protocol::checkXor( char *recvbuf,  int recvlen)
+   {
+     int i = 0;
+     char check = 0;
+     char *p = recvbuf;
+     int len;
+     if(*p == (char)0x02)
+     {
+       p = p+8;
+       len = recvlen - 9;
+       for(i = 0; i < len; i++)
+       {
+         check ^= *p++;
+       }
+       if(check == *p)
+       {
+         return true;
+       }
+       else
+         return false;
+     }
+     else
+     {
+       return false;
+     }
+   }
+
+}

src/wj_716_lidar/src/wj_716_lidar_protocol.h

@@ -0,0 +1,54 @@
+#ifndef WJ_716_LIDAR_PROTOCOL_H
+#define WJ_716_LIDAR_PROTOCOL_H
+#include <iostream>
+#include "string.h"
+#include <boost/shared_ptr.hpp>
+#include <boost/asio.hpp>
+#include <boost/asio/placeholders.hpp>
+#include <boost/system/error_code.hpp>
+#include <boost/bind/bind.hpp>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <sensor_msgs/LaserScan.h>
+#include <dynamic_reconfigure/server.h>
+#include <wj_716_lidar/wj_716_lidarConfig.h>
+using namespace std ;
+namespace wj_lidar
+{
+  #define MAX_LENGTH_DATA_PROCESS 200000
+  typedef struct TagDataCache
+  {
+    char m_acdata[MAX_LENGTH_DATA_PROCESS];
+    unsigned int m_u32in;
+    unsigned int m_u32out;
+  }DataCache;
+  class wj_716_lidar_protocol
+  {
+  public:
+    wj_716_lidar_protocol();
+    bool dataProcess(const char *data,const int reclen);
+    bool protocl(const char *data,const int len);
+    bool OnRecvProcess(char *data, int len);
+    bool checkXor(char *recvbuf, int recvlen);
+    void send_scan(const char *data,const int len);
+    ros::NodeHandle nh;
+    ros::Publisher marker_pub;                       //saomiao shujufabu
+    ros::Publisher twoecho_;                         //er chong huibo shuju
+    sensor_msgs::LaserScan scan;
+    sensor_msgs::LaserScan scan_TwoEcho;
+    bool setConfig(wj_716_lidar::wj_716_lidarConfig &new_config,uint32_t level);
+    //dynamic_reconfigure::Server<wj_safety_lidar_protocl::> dr_srv;
+    bool heartstate;
+  private:
+    char        data_[MAX_LENGTH_DATA_PROCESS];
+    DataCache   m_sdata;
+    wj_716_lidar::wj_716_lidarConfig config_;
+    unsigned int g_u32PreFrameNo;
+    float scandata[811];
+    float scandata_te[811];
+    float scaninden[811];
+
+  };
+
+}
+#endif // WJ_716_LIDAR_PROTOCOL_H

src/wj_716_lidar/src/wj_716_lidar_protocol.h.autosave

@@ -0,0 +1,54 @@
+#ifndef WJ_716_LIDAR_PROTOCOL_H
+#define WJ_716_LIDAR_PROTOCOL_H
+#include <iostream>
+#include "string.h"
+#include <boost/shared_ptr.hpp>
+#include <boost/asio.hpp>
+#include <boost/asio/placeholders.hpp>
+#include <boost/system/error_code.hpp>
+#include <boost/bind/bind.hpp>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <sensor_msgs/LaserScan.h>
+#include <dynamic_reconfigure/server.h>
+#include <wj_716_lidar/wj_716_lidarConfig.h>
+using namespace std ;
+namespace wj_lidar
+{
+  #define MAX_LENGTH_DATA_PROCESS 200000
+  typedef struct TagDataCachez
+  {
+    char m_acdata[MAX_LENGTH_DATA_PROCESS];
+    unsigned int m_u32in;
+    unsigned int m_u32out;
+  }DataCache;
+  class wj_716_lidar_protocol
+  {
+  public:
+    wj_716_lidar_protocol();
+    bool dataProcess(const char *data,const int reclen);
+    bool protocl(const char *data,const int len);
+    bool OnRecvProcess(char *data, int len);
+    bool checkXor(char *recvbuf, int recvlen);
+    void send_scan(const char *data,const int len);
+    ros::NodeHandle nh;
+    ros::Publisher marker_pub;                       //saomiao shujufabu
+    ros::Publisher twoecho_;                         //er chong huibo shuju
+    sensor_msgs::LaserScan scan;
+    sensor_msgs::LaserScan scan_TwoEcho;
+    bool setConfig(wj_716_lidar::wj_716_lidarConfig &new_config,uint32_t level);
+    //dynamic_reconfigure::Server<wj_safety_lidar_protocl::> dr_srv;
+    bool heartstate;
+  private:
+    char        data_[MAX_LENGTH_DATA_PROCESS];
+    DataCache   m_sdata;
+    wj_716_lidar::wj_716_lidarConfig config_;
+    unsigned int g_u32PreFrameNo;
+    float scandata[811];
+    float scandata_te[811];
+    float scaninden[811];
+
+  };
+
+}
+#endif // WJ_716_LIDAR_PROTOCOL_H