├── .gitignore ├── CMakeLists.txt ├── README.md ├── launch ├── demo.launch ├── flash_lidar.launch ├── lidar.launch ├── lidar.rviz ├── lidar_rviz.launch └── lidar_view.launch ├── package.xml ├── src ├── event.h ├── flashgo.cpp ├── flashgo.h ├── flashgo_client.cpp ├── flashgo_node.cpp └── locker.h └── startup └── initenv.sh /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flashgo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rosconsole 9 | roscpp 10 | sensor_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # sensor_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES flashgo 105 | # CATKIN_DEPENDS rosconsole roscpp sensor_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS}/src 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(flashgo 122 | # src/${PROJECT_NAME}/flashgo.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(flashgo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(flashgo_node src/flashgo_node.cpp) 132 | 133 | add_executable(flashgo_node src/flashgo_node.cpp src/flashgo.cpp) 134 | add_executable(flashgo_client src/flashgo_client.cpp) 135 | 136 | ## Add cmake target dependencies of the executable 137 | ## same as for the library above 138 | # add_dependencies(flashgo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Specify libraries to link a library or executable target against 141 | # target_link_libraries(flashgo_node 142 | # ${catkin_LIBRARIES} 143 | # ) 144 | 145 | target_link_libraries(flashgo_node 146 | ${catkin_LIBRARIES} 147 | ) 148 | target_link_libraries(flashgo_client 149 | ${catkin_LIBRARIES} 150 | ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # install(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables and/or libraries for installation 167 | # install(TARGETS flashgo flashgo_node 168 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark cpp header files for installation 174 | # install(DIRECTORY include/${PROJECT_NAME}/ 175 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 176 | # FILES_MATCHING PATTERN "*.h" 177 | # PATTERN ".svn" EXCLUDE 178 | # ) 179 | 180 | ## Mark other files for installation (e.g. launch and bag files, etc.) 181 | # install(FILES 182 | # # myfile1 183 | # # myfile2 184 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 185 | # ) 186 | 187 | ############# 188 | ## Testing ## 189 | ############# 190 | 191 | ## Add gtest based cpp test target and link libraries 192 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_flashgo.cpp) 193 | # if(TARGET ${PROJECT_NAME}-test) 194 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 195 | # endif() 196 | 197 | ## Add folders to be run by python nosetests 198 | # catkin_add_nosetests(test) 199 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | FLASH LIDAR ROS PACKAGE V2.3.9 2 | ===================================================================== 3 | 4 | ROS node and test application for FLASH LIDAR 5 | 6 | Visit EAI Website for more details about FLASH LIDAR. 7 | 8 | How to build FLASH LIDAR ros package 9 | ===================================================================== 10 | 1) Clone this project to your catkin's workspace src folder 11 | 2) Running catkin_make to build flashgo_node and flashgo_client 12 | 3) Create the name "/dev/flashlidar" for flash lidar 13 | --$ roscd flashgo/startup 14 | --$ sudo chmod 777 ./* 15 | --$ sudo sh initenv.sh 16 | 17 | How to run FLASH LIDAR ros package 18 | ===================================================================== 19 | There're two ways to run FLASH LIDAR ros package 20 | 21 | 1. Run FLASH LIDAR node and view in the rviz 22 | ------------------------------------------------------------ 23 | roslaunch flashgo lidar_view.launch 24 | 25 | You should see FLASH LIDAR's scan result in the rviz. 26 | 27 | 2. Run FLASH LIDAR node and view using test application 28 | ------------------------------------------------------------ 29 | roslaunch flashgo lidar.launch 30 | 31 | rosrun flashgo flashgo_client 32 | 33 | You should see FLASH LIDAR's scan result in the console 34 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/flash_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | -------------------------------------------------------------------------------- /launch/lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | -------------------------------------------------------------------------------- /launch/lidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LidarLaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 413 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Autocompute Intensity Bounds: true 53 | Autocompute Value Bounds: 54 | Max Value: 0 55 | Min Value: 0 56 | Value: true 57 | Axis: Z 58 | Channel Name: intensity 59 | Class: rviz/LaserScan 60 | Color: 255; 255; 255 61 | Color Transformer: AxisColor 62 | Decay Time: 0 63 | Enabled: true 64 | Invert Rainbow: false 65 | Max Color: 255; 255; 255 66 | Max Intensity: 4096 67 | Min Color: 0; 0; 0 68 | Min Intensity: 0 69 | Name: LidarLaserScan 70 | Position Transformer: XYZ 71 | Queue Size: 1000 72 | Selectable: true 73 | Size (Pixels): 5 74 | Size (m): 0.03 75 | Style: Squares 76 | Topic: /scan 77 | Use Fixed Frame: true 78 | Use rainbow: true 79 | Value: true 80 | Enabled: true 81 | Global Options: 82 | Background Color: 48; 48; 48 83 | Fixed Frame: laser_frame 84 | Frame Rate: 30 85 | Name: root 86 | Tools: 87 | - Class: rviz/MoveCamera 88 | - Class: rviz/Interact 89 | Hide Inactive Objects: true 90 | - Class: rviz/Select 91 | - Class: rviz/SetInitialPose 92 | Topic: /initialpose 93 | - Class: rviz/SetGoal 94 | Topic: /move_base_simple/goal 95 | Value: true 96 | Views: 97 | Current: 98 | Class: rviz/Orbit 99 | Distance: 11.1184 100 | Focal Point: 101 | X: -0.0344972 102 | Y: 0.065886 103 | Z: 0.148092 104 | Name: Current View 105 | Near Clip Distance: 0.01 106 | Pitch: 0.615399 107 | Target Frame: 108 | Value: Orbit (rviz) 109 | Yaw: 5.82358 110 | Saved: ~ 111 | Window Geometry: 112 | Displays: 113 | collapsed: false 114 | Height: 626 115 | Hide Left Dock: false 116 | Hide Right Dock: false 117 | QMainWindow State: 000000ff00000000fd0000000400000000000001950000022cfc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000022c000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003240000022c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 118 | Selection: 119 | collapsed: false 120 | Time: 121 | collapsed: false 122 | Tool Properties: 123 | collapsed: false 124 | Views: 125 | collapsed: false 126 | Width: 1215 127 | X: 65 128 | Y: 24 129 | -------------------------------------------------------------------------------- /launch/lidar_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/lidar_view.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flashgo 4 | 2.3.9 5 | The flashgo package 6 | 7 | 8 | 9 | 10 | shao 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rosconsole 44 | roscpp 45 | sensor_msgs 46 | rosconsole 47 | roscpp 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /src/event.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | class Event 5 | { 6 | public: 7 | 8 | enum 9 | { 10 | EVENT_OK = 1, 11 | EVENT_TIMEOUT = 2, 12 | EVENT_FAILED = 0, 13 | }; 14 | 15 | Event(bool isAutoReset = true, bool isSignal = false) 16 | #ifdef _WIN32 17 | : _event(NULL) 18 | #else 19 | : _is_signalled(isSignal) 20 | , _isAutoReset(isAutoReset) 21 | #endif 22 | { 23 | #ifdef _WIN32 24 | _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); 25 | #else 26 | pthread_mutex_init(&_cond_locker, NULL); 27 | pthread_cond_init(&_cond_var, NULL); 28 | #endif 29 | } 30 | 31 | ~ Event() 32 | { 33 | release(); 34 | } 35 | 36 | void set( bool isSignal = true ) 37 | { 38 | if (isSignal){ 39 | #ifdef _WIN32 40 | SetEvent(_event); 41 | #else 42 | pthread_mutex_lock(&_cond_locker); 43 | 44 | if ( _is_signalled == false ) 45 | { 46 | _is_signalled = true; 47 | pthread_cond_signal(&_cond_var); 48 | } 49 | pthread_mutex_unlock(&_cond_locker); 50 | #endif 51 | } 52 | else 53 | { 54 | #ifdef _WIN32 55 | ResetEvent(_event); 56 | #else 57 | pthread_mutex_lock(&_cond_locker); 58 | _is_signalled = false; 59 | pthread_mutex_unlock(&_cond_locker); 60 | #endif 61 | } 62 | } 63 | 64 | unsigned long wait( unsigned long timeout = 0xFFFFFFFF ) 65 | { 66 | #ifdef _WIN32 67 | switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) 68 | { 69 | case WAIT_FAILED: 70 | return EVENT_FAILED; 71 | case WAIT_OBJECT_0: 72 | return EVENT_OK; 73 | case WAIT_TIMEOUT: 74 | return EVENT_TIMEOUT; 75 | } 76 | return EVENT_OK; 77 | #else 78 | unsigned long ans = EVENT_OK; 79 | pthread_mutex_lock( &_cond_locker ); 80 | 81 | if ( !_is_signalled ) 82 | { 83 | 84 | if (timeout == 0xFFFFFFFF){ 85 | pthread_cond_wait(&_cond_var,&_cond_locker); 86 | }else 87 | { 88 | timespec wait_time; 89 | timeval now; 90 | gettimeofday(&now,NULL); 91 | 92 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 93 | wait_time.tv_nsec = (timeout%1000)*1000000ULL + now.tv_usec*1000; 94 | 95 | if (wait_time.tv_nsec >= 1000000000) 96 | { 97 | ++wait_time.tv_sec; 98 | wait_time.tv_nsec -= 1000000000; 99 | } 100 | switch (pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time)) 101 | { 102 | case 0: 103 | // signalled 104 | break; 105 | case ETIMEDOUT: 106 | // time up 107 | ans = EVENT_TIMEOUT; 108 | goto _final; 109 | break; 110 | default: 111 | ans = EVENT_FAILED; 112 | goto _final; 113 | } 114 | 115 | } 116 | } 117 | 118 | assert(_is_signalled); 119 | 120 | if ( _isAutoReset ) 121 | { 122 | _is_signalled = false; 123 | } 124 | _final: 125 | pthread_mutex_unlock( &_cond_locker ); 126 | 127 | return ans; 128 | #endif 129 | 130 | } 131 | protected: 132 | 133 | void release() 134 | { 135 | #ifdef _WIN32 136 | CloseHandle(_event); 137 | #else 138 | pthread_mutex_destroy(&_cond_locker); 139 | pthread_cond_destroy(&_cond_var); 140 | #endif 141 | } 142 | 143 | #ifdef _WIN32 144 | HANDLE _event; 145 | #else 146 | pthread_cond_t _cond_var; 147 | pthread_mutex_t _cond_locker; 148 | bool _is_signalled; 149 | bool _isAutoReset; 150 | #endif 151 | }; 152 | -------------------------------------------------------------------------------- /src/flashgo.cpp: -------------------------------------------------------------------------------- 1 | #include "flashgo.h" 2 | #include "event.h" 3 | #include "locker.h" 4 | 5 | static int serial_fd; 6 | static pthread_t threadId; 7 | size_t required_tx_cnt; 8 | size_t required_rx_cnt; 9 | u_int32_t _baudrate; 10 | 11 | Flashgo* Flashgo::_impl = NULL; 12 | 13 | Flashgo::Flashgo() 14 | { 15 | isConnected = false; 16 | isScanning = false; 17 | isThreadOn = false; 18 | } 19 | 20 | Flashgo::~Flashgo() 21 | { 22 | { 23 | ScopedLocker l(_scanning_lock); 24 | isScanning = false; 25 | } 26 | 27 | if(isThreadOn||threadId){ 28 | if(threadId) 29 | pthread_join(threadId , NULL); 30 | } 31 | } 32 | 33 | 34 | int Flashgo::connect(const char * port_path, u_int32_t baudrate) 35 | { 36 | _baudrate = baudrate; 37 | if (isConnected){ 38 | close(serial_fd); 39 | } 40 | 41 | { 42 | ScopedLocker l(_lock); 43 | if (serial_fd != -1) { 44 | close(serial_fd); 45 | } 46 | 47 | serial_fd = -1; 48 | serial_fd = open(port_path, O_RDWR | O_NOCTTY | O_NDELAY); 49 | if (serial_fd == -1) { 50 | return -1; 51 | } 52 | 53 | struct termios options, oldopt; 54 | tcgetattr(serial_fd, &oldopt); 55 | bzero(&options,sizeof(struct termios)); 56 | 57 | u_int32_t termbaud = getTermBaudBitmap(baudrate); 58 | if (termbaud == (u_int32_t)-1) { 59 | if (serial_fd != -1) { 60 | close(serial_fd); 61 | } 62 | serial_fd = -1; 63 | return -1; 64 | } 65 | cfsetispeed(&options, termbaud); 66 | cfsetospeed(&options, termbaud); 67 | 68 | options.c_cflag |= (CLOCAL | CREAD); 69 | options.c_cflag &= ~PARENB; 70 | options.c_cflag &= ~CSTOPB; 71 | options.c_cflag &= ~CSIZE; 72 | options.c_cflag |= CS8; 73 | options.c_iflag &= ~(IXON | IXOFF | IXANY); 74 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 75 | options.c_oflag &= ~OPOST; 76 | 77 | tcflush(serial_fd,TCIFLUSH); 78 | if (fcntl(serial_fd, F_SETFL, FNDELAY)) { 79 | if (serial_fd != -1) { 80 | close(serial_fd); 81 | } 82 | serial_fd = -1; 83 | return -1; 84 | } 85 | if (tcsetattr(serial_fd, TCSANOW, &options)) { 86 | if (serial_fd != -1) { 87 | close(serial_fd); 88 | } 89 | serial_fd = -1; 90 | return -1; 91 | } 92 | 93 | u_int32_t dtr_bit = TIOCM_DTR; 94 | ioctl(serial_fd, TIOCMBIC, &dtr_bit); 95 | } 96 | 97 | isConnected = true; 98 | return serial_fd; 99 | } 100 | 101 | 102 | void Flashgo::disconnect() 103 | { 104 | if (!isConnected){ 105 | return ; 106 | } 107 | stop(); 108 | if (serial_fd != -1) { 109 | int ret; 110 | ret = close(serial_fd); 111 | if(ret == 0){ 112 | serial_fd = -1; 113 | }else{ 114 | THROW (IOException, errno); 115 | } 116 | } 117 | isConnected = false; 118 | serial_fd = -1; 119 | } 120 | 121 | u_int32_t Flashgo::getTermBaudBitmap(u_int32_t baud) 122 | { 123 | #define BAUD_CONV( _baud_) case _baud_: return B##_baud_ 124 | switch (baud) { 125 | BAUD_CONV(1200); 126 | BAUD_CONV(1800); 127 | BAUD_CONV(2400); 128 | BAUD_CONV(4800); 129 | BAUD_CONV(9600); 130 | BAUD_CONV(19200); 131 | BAUD_CONV(38400); 132 | BAUD_CONV(57600); 133 | BAUD_CONV(115200); 134 | BAUD_CONV(230400); 135 | BAUD_CONV(460800); 136 | BAUD_CONV(500000); 137 | BAUD_CONV(576000); 138 | BAUD_CONV(921600); 139 | BAUD_CONV(1000000); 140 | BAUD_CONV(1152000); 141 | BAUD_CONV(1500000); 142 | BAUD_CONV(2000000); 143 | BAUD_CONV(2500000); 144 | BAUD_CONV(3000000); 145 | BAUD_CONV(3500000); 146 | BAUD_CONV(4000000); 147 | } 148 | return -1; 149 | } 150 | 151 | void Flashgo::disableDataGrabbing() 152 | { 153 | { 154 | ScopedLocker l(_scanning_lock); 155 | isScanning = false; 156 | } 157 | 158 | if(isThreadOn){ 159 | if(threadId){ 160 | pthread_join(threadId , NULL); 161 | } 162 | } 163 | } 164 | 165 | int Flashgo::sendCommand(u_int8_t cmd, const void * payload, size_t payloadsize) 166 | { 167 | u_int8_t pkt_header[10]; 168 | cmd_packet * header = reinterpret_cast(pkt_header); 169 | u_int8_t checksum = 0; 170 | 171 | if (!isConnected) { 172 | return -2; 173 | } 174 | if (payloadsize && payload) { 175 | cmd |= LIDAR_CMDFLAG_HAS_PAYLOAD; 176 | } 177 | 178 | header->syncByte = LIDAR_CMD_SYNC_BYTE; 179 | header->cmd_flag = cmd; 180 | sendData(pkt_header, 2) ; 181 | 182 | if (cmd & LIDAR_CMDFLAG_HAS_PAYLOAD) { 183 | checksum ^= LIDAR_CMD_SYNC_BYTE; 184 | checksum ^= cmd; 185 | checksum ^= (payloadsize & 0xFF); 186 | 187 | for (size_t pos = 0; pos < payloadsize; ++pos) { 188 | checksum ^= ((u_int8_t *)payload)[pos]; 189 | } 190 | 191 | u_int8_t sizebyte = payloadsize; 192 | sendData(&sizebyte, 1); 193 | 194 | sendData((const u_int8_t *)payload, sizebyte); 195 | 196 | sendData(&checksum, 1); 197 | } 198 | return 0; 199 | } 200 | 201 | int Flashgo::sendData(const unsigned char * data, size_t size) 202 | { 203 | if (!isConnected) { 204 | return 0; 205 | } 206 | 207 | if (data == NULL || size ==0) { 208 | return 0; 209 | } 210 | 211 | size_t tx_len = 0; 212 | required_tx_cnt = 0; 213 | do { 214 | int ans = write(serial_fd, data + tx_len, size-tx_len); 215 | if (ans == -1) { 216 | return tx_len; 217 | } 218 | tx_len += ans; 219 | required_tx_cnt = tx_len; 220 | }while (tx_len(header); 251 | u_int32_t waitTime; 252 | 253 | while ((waitTime=getms() - startTs) <= timeout) { 254 | size_t remainSize = sizeof(lidar_ans_header) - recvPos; 255 | size_t recvSize; 256 | 257 | int ans = waitForData(remainSize, timeout - waitTime, &recvSize); 258 | if (ans == -2){ 259 | return -2; 260 | }else if (ans == -1){ 261 | return -1; 262 | } 263 | 264 | if(recvSize > remainSize) recvSize = remainSize; 265 | 266 | ans = getData(recvBuffer, recvSize); 267 | if (ans == -1){ 268 | return -2; 269 | } 270 | 271 | for (size_t pos = 0; pos < recvSize; ++pos) { 272 | u_int8_t currentByte = recvBuffer[pos]; 273 | switch (recvPos) { 274 | case 0: 275 | if (currentByte != LIDAR_ANS_SYNC_BYTE1) { 276 | continue; 277 | } 278 | break; 279 | case 1: 280 | if (currentByte != LIDAR_ANS_SYNC_BYTE2) { 281 | recvPos = 0; 282 | continue; 283 | } 284 | break; 285 | } 286 | headerBuffer[recvPos++] = currentByte; 287 | 288 | if (recvPos == sizeof(lidar_ans_header)) { 289 | return 0; 290 | } 291 | } 292 | } 293 | return -1; 294 | } 295 | 296 | int Flashgo::waitForData(size_t data_count, u_int32_t timeout, size_t * returned_size) 297 | { 298 | size_t length = 0; 299 | if (returned_size==NULL) { 300 | returned_size=(size_t *)&length; 301 | } 302 | 303 | int max_fd; 304 | fd_set input_set; 305 | struct timeval timeout_val; 306 | 307 | FD_ZERO(&input_set); 308 | FD_SET(serial_fd, &input_set); 309 | max_fd = serial_fd + 1; 310 | 311 | timeout_val.tv_sec = timeout / 1000; 312 | timeout_val.tv_usec = (timeout % 1000) * 1000; 313 | 314 | if ( isConnected ){ 315 | if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { 316 | THROW (IOException, errno); 317 | return -2; 318 | } 319 | if (*returned_size >= data_count){ 320 | return 0; 321 | } 322 | } 323 | 324 | while (isConnected) { 325 | int n = select(max_fd, &input_set, NULL, NULL, &timeout_val); 326 | if (n < 0){ 327 | // Select was interrupted 328 | if (errno == EINTR) { 329 | return -1; 330 | } 331 | // Otherwise there was some error 332 | THROW (IOException, errno); 333 | return -2; 334 | }else if (n == 0) { 335 | return -1; 336 | } else { 337 | assert (FD_ISSET(serial_fd, &input_set)); 338 | if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { 339 | THROW (IOException, errno); 340 | return -2; 341 | } 342 | 343 | if (*returned_size >= data_count){ 344 | return 0; 345 | } else { 346 | int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec; 347 | int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate; 348 | if (remain_timeout > expect_remain_time){ 349 | usleep(expect_remain_time); 350 | } 351 | } 352 | } 353 | } 354 | THROW (IOException, errno); 355 | return -2; 356 | } 357 | 358 | int Flashgo::getHealth(device_health & health, u_int32_t timeout) 359 | { 360 | int ans; 361 | if (!isConnected) { 362 | return -2; 363 | } 364 | 365 | disableDataGrabbing(); 366 | { 367 | if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_HEALTH)) != 0) { 368 | return ans; 369 | } 370 | lidar_ans_header response_header; 371 | if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { 372 | return ans; 373 | } 374 | 375 | if (response_header.type != LIDAR_ANS_TYPE_DEVHEALTH) { 376 | return -3; 377 | } 378 | 379 | if (response_header.size < sizeof(device_health)) { 380 | return -3; 381 | } 382 | 383 | if (waitForData(response_header.size, timeout) != 0) { 384 | return -1; 385 | } 386 | 387 | getData(reinterpret_cast(&health), sizeof(health)); 388 | } 389 | return 0; 390 | } 391 | 392 | 393 | int Flashgo::getDeviceInfo(device_info & info, u_int32_t timeout) 394 | { 395 | int ans; 396 | if (!isConnected) { 397 | return -2; 398 | } 399 | 400 | disableDataGrabbing(); 401 | { 402 | ScopedLocker l(_lock); 403 | if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_INFO)) != 0) { 404 | return ans; 405 | } 406 | 407 | lidar_ans_header response_header; 408 | if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { 409 | return ans; 410 | } 411 | 412 | if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) { 413 | return -3; 414 | } 415 | 416 | if (response_header.size < sizeof(lidar_ans_header)) { 417 | return -3; 418 | } 419 | 420 | if (waitForData(response_header.size, timeout) != 0) { 421 | return -1; 422 | } 423 | getData(reinterpret_cast(&info), sizeof(info)); 424 | } 425 | return 0; 426 | } 427 | 428 | int Flashgo::startScan(bool force, u_int32_t timeout ) 429 | { 430 | int ans; 431 | if (!isConnected) { 432 | return -2; 433 | } 434 | if (isScanning) { 435 | return 0; 436 | } 437 | 438 | stop(); 439 | { 440 | ScopedLocker l(_lock); 441 | if ((ans = sendCommand(force?LIDAR_CMD_FORCE_SCAN:LIDAR_CMD_SCAN)) != 0) { 442 | return ans; 443 | } 444 | 445 | lidar_ans_header response_header; 446 | if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { 447 | return ans; 448 | } 449 | 450 | if (response_header.type != LIDAR_ANS_TYPE_MEASUREMENT) { 451 | return -3; 452 | } 453 | 454 | if (response_header.size < sizeof(node_info)) { 455 | return -3; 456 | } 457 | 458 | isScanning = true; 459 | ans = this->createThread(); 460 | return ans; 461 | } 462 | return 0; 463 | } 464 | 465 | void * Flashgo::_thread_t(void *args) 466 | { 467 | Flashgo* pThis = static_cast(args); 468 | pThis->cacheScanData(); 469 | return NULL; 470 | } 471 | 472 | int Flashgo::createThread() 473 | { 474 | if(pthread_create(&threadId,NULL,_thread_t,(void *)this) != 0) { 475 | isThreadOn = false; 476 | return -2; 477 | } 478 | isThreadOn = true; 479 | return 0; 480 | } 481 | 482 | int Flashgo::stop() 483 | { 484 | int ans; 485 | node_info local_buf[128]; 486 | size_t count = 128; 487 | 488 | disableDataGrabbing(); 489 | { 490 | ScopedLocker l(_lock); 491 | ans = sendCommand(LIDAR_CMD_FORCE_STOP); 492 | if(ans != 0) 493 | return ans; 494 | } 495 | 496 | ScopedLocker l(_lock); 497 | while(true) { 498 | if ((ans = waitScanData(local_buf, count ,10)) != 0 ) { 499 | if (ans == -1) { 500 | break; 501 | } 502 | }else{ 503 | sendCommand(LIDAR_CMD_FORCE_STOP); 504 | } 505 | } 506 | sendCommand(LIDAR_CMD_STOP); 507 | return 0; 508 | } 509 | 510 | int Flashgo::cacheScanData() 511 | { 512 | node_info local_buf[128]; 513 | size_t count = 128; 514 | node_info local_scan[MAX_SCAN_NODES]; 515 | size_t scan_count = 0; 516 | int ans; 517 | memset(local_scan, 0, sizeof(local_scan)); 518 | 519 | waitScanData(local_buf, count); 520 | 521 | while(isScanning) { 522 | if ((ans=waitScanData(local_buf, count)) != 0) { 523 | if (ans != -1) { 524 | fprintf(stderr, "exit scanning thread!!"); 525 | { 526 | ScopedLocker l(_scanning_lock); 527 | isScanning = false; 528 | } 529 | return -2; 530 | } 531 | } 532 | 533 | for (size_t pos = 0; pos < count; ++pos) { 534 | if (local_buf[pos].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT) { 535 | if ((local_scan[0].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT)) { 536 | _lock.lock();//timeout lock, wait resource copy 537 | memcpy(scan_node_buf, local_scan, scan_count*sizeof(node_info)); 538 | scan_node_count = scan_count; 539 | _dataEvt.set(); 540 | _lock.unlock(); 541 | } 542 | scan_count = 0; 543 | } 544 | local_scan[scan_count++] = local_buf[pos]; 545 | if (scan_count == _countof(local_scan)) scan_count-=1; 546 | } 547 | 548 | 549 | } 550 | { 551 | ScopedLocker l(_scanning_lock); 552 | isScanning = false; 553 | } 554 | return 0; 555 | } 556 | 557 | int Flashgo::waitPackage(node_info * node, u_int32_t timeout) 558 | { 559 | int recvPos = 0; 560 | int recvPosEnd = 0; 561 | u_int32_t startTs = getms(); 562 | u_int8_t recvBuffer[sizeof(node_package)]; 563 | u_int8_t *nodeBuffer = (u_int8_t*)node; 564 | u_int32_t waitTime; 565 | 566 | static node_package package; 567 | static u_int16_t package_Sample_Index = 0; 568 | static u_int16_t IntervalSampleAngle = 0; 569 | static u_int16_t IntervalSampleAngle_LastPackage = 0; 570 | static u_int16_t FirstSampleAngle = 0; 571 | static u_int16_t LastSampleAngle = 0; 572 | static u_int16_t CheckSun = 0; 573 | static u_int32_t RingNum = 0; 574 | 575 | static u_int16_t CheckSunCal = 0; 576 | static u_int16_t SampleNumlAndCTCal = 0; 577 | static u_int16_t LastSampleAngleCal = 0; 578 | static bool CheckSunResult = true; 579 | static u_int16_t Valu8Tou16 = 0; 580 | 581 | u_int8_t *packageBuffer = (u_int8_t*)&package.package_Head; 582 | u_int8_t package_Sample_Num = 0; 583 | 584 | int32_t AngleCorrectForDistance; 585 | 586 | int package_recvPos = 0; 587 | 588 | if(package_Sample_Index == 0) { 589 | recvPos = 0; 590 | while ((waitTime=getms() - startTs) <= timeout) { 591 | size_t remainSize = PackagePaidBytes - recvPos; 592 | size_t recvSize; 593 | int ans = waitForData(remainSize, timeout-waitTime, &recvSize); 594 | if (ans == -2){ 595 | return -2; 596 | }else if (ans == -1){ 597 | return -1; 598 | } 599 | 600 | if (recvSize > remainSize) recvSize = remainSize; 601 | 602 | getData(recvBuffer, recvSize); 603 | 604 | for (size_t pos = 0; pos < recvSize; ++pos) { 605 | u_int8_t currentByte = recvBuffer[pos]; 606 | switch (recvPos) { 607 | case 0: 608 | if ( currentByte == (PH&0xFF) ) { 609 | } else { 610 | continue; 611 | } 612 | break; 613 | case 1: 614 | CheckSunCal = PH; 615 | if ( currentByte == (PH>>8) ) { 616 | } else { 617 | recvPos = 0; 618 | continue; 619 | } 620 | break; 621 | case 2: 622 | SampleNumlAndCTCal = currentByte; 623 | if ((currentByte == CT_Normal) || (currentByte == CT_RingStart)){ 624 | } else { 625 | recvPos = 0; 626 | continue; 627 | } 628 | break; 629 | case 3: 630 | SampleNumlAndCTCal += (currentByte*0x100); 631 | package_Sample_Num = currentByte; 632 | break; 633 | case 4: 634 | if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { 635 | FirstSampleAngle = currentByte; 636 | } else { 637 | recvPos = 0; 638 | continue; 639 | } 640 | break; 641 | case 5: 642 | FirstSampleAngle += currentByte*0x100; 643 | CheckSunCal ^= FirstSampleAngle; 644 | FirstSampleAngle = FirstSampleAngle>>1; 645 | break; 646 | case 6: 647 | if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { 648 | LastSampleAngle = currentByte; 649 | } else { 650 | recvPos = 0; 651 | continue; 652 | } 653 | break; 654 | case 7: 655 | LastSampleAngle = currentByte*0x100 + LastSampleAngle; 656 | LastSampleAngleCal = LastSampleAngle; 657 | LastSampleAngle = LastSampleAngle>>1; 658 | if(package_Sample_Num == 1){ 659 | IntervalSampleAngle = 0; 660 | }else{ 661 | if(LastSampleAngle < FirstSampleAngle){ 662 | if((FirstSampleAngle > 270*64) && (LastSampleAngle < 90*64)){ 663 | IntervalSampleAngle = (360*64 + LastSampleAngle - FirstSampleAngle)/(package_Sample_Num-1); 664 | IntervalSampleAngle_LastPackage = IntervalSampleAngle; 665 | } else{ 666 | IntervalSampleAngle = IntervalSampleAngle_LastPackage; 667 | } 668 | } else{ 669 | IntervalSampleAngle = (LastSampleAngle -FirstSampleAngle)/(package_Sample_Num-1); 670 | IntervalSampleAngle_LastPackage = IntervalSampleAngle; 671 | } 672 | } 673 | break; 674 | case 8: 675 | CheckSun = currentByte; 676 | break; 677 | case 9: 678 | CheckSun += (currentByte*0x100); 679 | break; 680 | } 681 | packageBuffer[recvPos++] = currentByte; 682 | } 683 | 684 | if (recvPos == PackagePaidBytes ){ 685 | package_recvPos = recvPos; 686 | break; 687 | } 688 | } 689 | 690 | if(PackagePaidBytes == recvPos){ 691 | startTs = getms(); 692 | recvPos = 0; 693 | while ((waitTime=getms() - startTs) <= timeout) { 694 | size_t remainSize = package_Sample_Num*PackageSampleBytes - recvPos; 695 | size_t recvSize; 696 | int ans =waitForData(remainSize, timeout-waitTime, &recvSize); 697 | if (ans == -2){ 698 | return -2; 699 | }else if (ans == -1){ 700 | return -1; 701 | } 702 | 703 | if (recvSize > remainSize) recvSize = remainSize; 704 | 705 | getData(recvBuffer, recvSize); 706 | 707 | for (size_t pos = 0; pos < recvSize; ++pos) { 708 | if(recvPos%2 == 1){ 709 | Valu8Tou16 += recvBuffer[pos]*0x100; 710 | CheckSunCal ^= Valu8Tou16; 711 | }else{ 712 | Valu8Tou16 = recvBuffer[pos]; 713 | } 714 | packageBuffer[package_recvPos+recvPos] = recvBuffer[pos]; 715 | recvPos++; 716 | } 717 | if(package_Sample_Num*PackageSampleBytes == recvPos){ 718 | package_recvPos += recvPos; 719 | break; 720 | } 721 | } 722 | if(package_Sample_Num*PackageSampleBytes != recvPos){ 723 | return -1; 724 | } 725 | } else { 726 | return -1; 727 | } 728 | CheckSunCal ^= SampleNumlAndCTCal; 729 | CheckSunCal ^= LastSampleAngleCal; 730 | 731 | if(CheckSunCal != CheckSun){ 732 | CheckSunResult = false; 733 | }else{ 734 | CheckSunResult = true; 735 | } 736 | 737 | } 738 | 739 | if(package.package_CT == CT_Normal){ 740 | (*node).sync_quality = Node_Default_Quality + Node_NotSync; 741 | } else{ 742 | (*node).sync_quality = Node_Default_Quality + Node_Sync; 743 | } 744 | 745 | if(CheckSunResult == true){ 746 | (*node).distance_q2 = package.packageSampleDistance[package_Sample_Index]; 747 | if((*node).distance_q2/4 != 0){ 748 | AngleCorrectForDistance = (int32_t)(((atan(((21.8*(155.3 - ((*node).distance_q2/4)) )/155.3)/((*node).distance_q2/4)))*180.0/3.1415) * 64.0); 749 | }else{ 750 | AngleCorrectForDistance = 0; 751 | } 752 | if((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance) < 0){ 753 | (*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance + 360*64)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT; 754 | }else{ 755 | if((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance) > 360*64){ 756 | (*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance - 360*64)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT; 757 | }else{ 758 | (*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT; 759 | } 760 | } 761 | }else{ 762 | (*node).sync_quality = Node_Default_Quality + Node_NotSync; 763 | (*node).angle_q6_checkbit = LIDAR_RESP_MEASUREMENT_CHECKBIT; 764 | (*node).distance_q2 = 0; 765 | } 766 | 767 | package_Sample_Index++; 768 | if(package_Sample_Index >= package.nowPackageNum){ 769 | package_Sample_Index = 0; 770 | } 771 | return 0; 772 | } 773 | 774 | int Flashgo::waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout) 775 | { 776 | if (!isConnected) { 777 | count = 0; 778 | return -2; 779 | } 780 | 781 | size_t recvNodeCount = 0; 782 | u_int32_t startTs = getms(); 783 | u_int32_t waitTime; 784 | int ans; 785 | 786 | while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { 787 | node_info node; 788 | if ((ans = this->waitPackage(&node, timeout - waitTime)) != 0) { 789 | return ans; 790 | } 791 | 792 | nodebuffer[recvNodeCount++] = node; 793 | 794 | if (recvNodeCount == count) { 795 | 796 | return 0; 797 | } 798 | } 799 | count = recvNodeCount; 800 | 801 | return -1; 802 | } 803 | 804 | int Flashgo::grabScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout) 805 | { 806 | switch (_dataEvt.wait(timeout)) 807 | { 808 | case Event::EVENT_TIMEOUT: 809 | count = 0; 810 | return -2; 811 | case Event::EVENT_OK: 812 | { 813 | if(scan_node_count == 0) { 814 | return -2; 815 | } 816 | size_t size_to_copy = min(count, scan_node_count); 817 | ScopedLocker l(_lock); 818 | memcpy(nodebuffer, scan_node_buf, size_to_copy*sizeof(node_info)); 819 | count = size_to_copy; 820 | scan_node_count = 0; 821 | } 822 | return 0; 823 | default: 824 | count = 0; 825 | return -1; 826 | } 827 | } 828 | 829 | void Flashgo::simpleScanData(std::vector *scan_data , node_info *buffer, size_t count) 830 | { 831 | scan_data->clear(); 832 | for (int pos = 0; pos < (int)count; ++pos) { 833 | scanDot dot; 834 | if (!buffer[pos].distance_q2) continue; 835 | dot.quality = (buffer[pos].sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); 836 | dot.angle = (buffer[pos].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; 837 | dot.dist = buffer[pos].distance_q2/4.0f; 838 | scan_data->push_back(dot); 839 | } 840 | } 841 | 842 | int Flashgo::ascendScanData(node_info * nodebuffer, size_t count) 843 | { 844 | float inc_origin_angle = 360.0/count; 845 | node_info *tmpbuffer = new node_info[count]; 846 | int i = 0; 847 | 848 | for (i = 0; i < (int)count; i++) { 849 | if(nodebuffer[i].distance_q2 == 0) { 850 | continue; 851 | } else { 852 | while(i != 0) { 853 | i--; 854 | float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle; 855 | if (expect_angle < 0.0f) expect_angle = 0.0f; 856 | u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; 857 | nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; 858 | } 859 | break; 860 | } 861 | } 862 | 863 | if (i == (int)count) return -3; 864 | 865 | for (i = (int)count - 1; i >= 0; i--) { 866 | if(nodebuffer[i].distance_q2 == 0) { 867 | continue; 868 | } else { 869 | while(i != ((int)count - 1)) { 870 | i++; 871 | float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle; 872 | if (expect_angle > 360.0f) expect_angle -= 360.0f; 873 | u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; 874 | nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; 875 | } 876 | break; 877 | } 878 | } 879 | 880 | float frontAngle = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; 881 | for (i = 1; i < (int)count; i++) { 882 | if(nodebuffer[i].distance_q2 == 0) { 883 | float expect_angle = frontAngle + i * inc_origin_angle; 884 | if (expect_angle > 360.0f) expect_angle -= 360.0f; 885 | u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; 886 | nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; 887 | } 888 | } 889 | 890 | size_t zero_pos = 0; 891 | float pre_degree = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; 892 | 893 | for (i = 1; i < (int)count ; ++i) { 894 | float degree = (nodebuffer[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; 895 | if (zero_pos == 0 && (pre_degree - degree > 180)) { 896 | zero_pos = i; 897 | break; 898 | } 899 | pre_degree = degree; 900 | } 901 | 902 | for (i = (int)zero_pos; i < (int)count; i++) { 903 | tmpbuffer[i-zero_pos] = nodebuffer[i]; 904 | } 905 | for (i = 0; i < (int)zero_pos; i++) { 906 | tmpbuffer[i+(int)count-zero_pos] = nodebuffer[i]; 907 | } 908 | 909 | memcpy(nodebuffer, tmpbuffer, count*sizeof(node_info)); 910 | delete[] tmpbuffer; 911 | 912 | return 0; 913 | } 914 | 915 | -------------------------------------------------------------------------------- /src/flashgo.h: -------------------------------------------------------------------------------- 1 | #ifndef FLASHGO_H 2 | #define FLASHGO_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "event.h" 38 | #include "locker.h" 39 | 40 | #define LIDAR_CMD_STOP 0x65 41 | #define LIDAR_CMD_SCAN 0x60 42 | #define LIDAR_CMD_FORCE_SCAN 0x61 43 | #define LIDAR_CMD_FORCE_STOP 0x00 44 | #define LIDAR_CMD_GET_EAI 0x55 45 | #define LIDAR_CMD_GET_DEVICE_INFO 0x90 46 | #define LIDAR_CMD_GET_DEVICE_HEALTH 0x92 47 | #define LIDAR_ANS_TYPE_DEVINFO 0x4 48 | #define LIDAR_ANS_TYPE_DEVHEALTH 0x6 49 | #define LIDAR_CMD_SYNC_BYTE 0xA5 50 | #define LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 51 | #define LIDAR_ANS_SYNC_BYTE1 0xA5 52 | #define LIDAR_ANS_SYNC_BYTE2 0x5A 53 | #define LIDAR_ANS_TYPE_MEASUREMENT 0x81 54 | #define LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) 55 | #define LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 56 | #define LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) 57 | #define LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 58 | 59 | #ifndef min 60 | #define min(a,b) (((a) < (b)) ? (a) : (b)) 61 | #endif 62 | 63 | #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) 64 | 65 | #define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \ 66 | __LINE__, (message) ) 67 | 68 | #define PackageSampleMaxLngth 0x100 69 | typedef enum 70 | { 71 | CT_Normal = 0, 72 | CT_RingStart = 1, 73 | CT_Tail, 74 | }CT; 75 | #define Node_Default_Quality (10<<2) 76 | #define Node_Sync 1 77 | #define Node_NotSync 2 78 | #define PackagePaidBytes 10 79 | #define PackageSampleBytes 2 80 | #define PH 0x55AA 81 | 82 | struct node_info{ 83 | u_int8_t sync_quality; 84 | u_int16_t angle_q6_checkbit; 85 | u_int16_t distance_q2; 86 | } __attribute__((packed)) ; 87 | 88 | struct node_package { 89 | u_int16_t package_Head; 90 | u_int8_t package_CT; 91 | u_int8_t nowPackageNum; 92 | u_int16_t packageFirstSampleAngle; 93 | u_int16_t packageLastSampleAngle; 94 | u_int16_t checkSum; 95 | u_int16_t packageSampleDistance[PackageSampleMaxLngth]; 96 | } __attribute__((packed)) ; 97 | 98 | struct device_info{ 99 | u_int8_t model; 100 | u_int16_t firmware_version; 101 | u_int8_t hardware_version; 102 | u_int8_t serialnum[16]; 103 | } __attribute__((packed)) ; 104 | 105 | struct device_health { 106 | u_int8_t status; 107 | u_int16_t error_code; 108 | } __attribute__((packed)) ; 109 | 110 | struct cmd_packet { 111 | u_int8_t syncByte; 112 | u_int8_t cmd_flag; 113 | u_int8_t size; 114 | u_int8_t data[0]; 115 | } __attribute__((packed)) ; 116 | 117 | struct lidar_ans_header { 118 | u_int8_t syncByte1; 119 | u_int8_t syncByte2; 120 | u_int32_t size:30; 121 | u_int32_t subType:2; 122 | u_int8_t type; 123 | } __attribute__((packed)); 124 | 125 | struct scanDot { 126 | u_int8_t quality; 127 | float angle; 128 | float dist; 129 | }; 130 | 131 | class Flashgo 132 | { 133 | public: 134 | static Flashgo* singleton() 135 | { 136 | return _impl; 137 | } 138 | static void initDriver() 139 | { 140 | _impl = new Flashgo; 141 | } 142 | static void done() 143 | { 144 | if(_impl){ 145 | //safe delete 146 | delete _impl; 147 | _impl = NULL; 148 | } 149 | } 150 | 151 | public: 152 | int connect(const char * port_path, u_int32_t baudrate); 153 | void disconnect(); 154 | int getHealth(device_health & health, u_int32_t timeout = DEFAULT_TIMEOUT); 155 | int getDeviceInfo(device_info & info, u_int32_t timeout = DEFAULT_TIMEOUT); 156 | int startScan(bool force = false, u_int32_t timeout = DEFAULT_TIMEOUT) ; 157 | int stop(); 158 | int grabScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout = DEFAULT_TIMEOUT) ; 159 | int ascendScanData(node_info * nodebuffer, size_t count); 160 | int createThread(); 161 | u_int32_t getms(); 162 | void simpleScanData(std::vector * scan_data , node_info *buffer, size_t count); 163 | 164 | protected: 165 | Flashgo(); 166 | virtual ~Flashgo(); 167 | int waitPackage(node_info * node, u_int32_t timeout = DEFAULT_TIMEOUT); 168 | int waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout = DEFAULT_TIMEOUT); 169 | int cacheScanData(); 170 | int sendCommand(u_int8_t cmd, const void * payload = NULL, size_t payloadsize = 0); 171 | int waitResponseHeader(lidar_ans_header * header, u_int32_t timeout = DEFAULT_TIMEOUT); 172 | u_int32_t getTermBaudBitmap(u_int32_t baud); 173 | int waitForData(size_t data_count,u_int32_t timeout = -1, size_t * returned_size = NULL); 174 | int getData(unsigned char * data, size_t size); 175 | int sendData(const unsigned char * data, size_t size); 176 | void disableDataGrabbing(); 177 | static void* _thread_t(void* args); 178 | 179 | public: 180 | bool isConnected; 181 | bool isScanning; 182 | bool isThreadOn; 183 | enum { 184 | DEFAULT_TIMEOUT = 2000, //2000 ms 185 | MAX_SCAN_NODES = 2048, 186 | }; 187 | node_info scan_node_buf[2048]; 188 | size_t scan_node_count; 189 | Locker _scanning_lock; 190 | Event _dataEvt; 191 | Locker _lock; 192 | 193 | static Flashgo* _impl; 194 | }; 195 | 196 | 197 | class SerialException : public std::exception 198 | { 199 | // Disable copy constructors 200 | SerialException& operator=(const SerialException&); 201 | std::string e_what_; 202 | public: 203 | SerialException (const char *description) { 204 | std::stringstream ss; 205 | ss << "SerialException " << description << " failed."; 206 | e_what_ = ss.str(); 207 | } 208 | SerialException (const SerialException& other) : e_what_(other.e_what_) {} 209 | virtual ~SerialException() throw() {} 210 | virtual const char* what () const throw () { 211 | return e_what_.c_str(); 212 | } 213 | }; 214 | 215 | class IOException : public std::exception 216 | { 217 | // Disable copy constructors 218 | IOException& operator=(const IOException&); 219 | std::string file_; 220 | int line_; 221 | std::string e_what_; 222 | int errno_; 223 | public: 224 | explicit IOException (std::string file, int line, int errnum) 225 | : file_(file), line_(line), errno_(errnum) { 226 | std::stringstream ss; 227 | #if defined(_WIN32) && !defined(__MINGW32__) 228 | char error_str [1024]; 229 | strerror_s(error_str, 1024, errnum); 230 | #else 231 | char * error_str = strerror(errnum); 232 | #endif 233 | ss << "IO Exception (" << errno_ << "): " << error_str; 234 | ss << ", file " << file_ << ", line " << line_ << "."; 235 | e_what_ = ss.str(); 236 | } 237 | explicit IOException (std::string file, int line, const char * description) 238 | : file_(file), line_(line), errno_(0) { 239 | std::stringstream ss; 240 | ss << "IO Exception: " << description; 241 | ss << ", file " << file_ << ", line " << line_ << "."; 242 | e_what_ = ss.str(); 243 | } 244 | virtual ~IOException() throw() {} 245 | IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {} 246 | 247 | int getErrorNumber () const { return errno_; } 248 | 249 | virtual const char* what () const throw () { 250 | return e_what_.c_str(); 251 | } 252 | }; 253 | 254 | class PortNotOpenedException : public std::exception 255 | { 256 | // Disable copy constructors 257 | const PortNotOpenedException& operator=(PortNotOpenedException); 258 | std::string e_what_; 259 | public: 260 | PortNotOpenedException (const char * description) { 261 | std::stringstream ss; 262 | ss << "PortNotOpenedException " << description << " failed."; 263 | e_what_ = ss.str(); 264 | } 265 | PortNotOpenedException (const PortNotOpenedException& other) : e_what_(other.e_what_) {} 266 | virtual ~PortNotOpenedException() throw() {} 267 | virtual const char* what () const throw () { 268 | return e_what_.c_str(); 269 | } 270 | 271 | }; 272 | 273 | #endif // FLASHGO_H 274 | -------------------------------------------------------------------------------- /src/flashgo_client.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * YDLIDAR SYSTEM 3 | * YDLIDAR ROS Node Client 4 | * 5 | * Copyright 2015 - 2017 EAI TEAM 6 | * http://www.eaibot.com 7 | * 8 | */ 9 | 10 | #include "ros/ros.h" 11 | #include "sensor_msgs/LaserScan.h" 12 | 13 | #define RAD2DEG(x) ((x)*180./M_PI) 14 | 15 | int round_double(double number) 16 | { 17 | return (number > 0.0) ? (number + 0.5) : (number - 0.5); 18 | } 19 | 20 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) 21 | { 22 | int count = round_double(scan->scan_time / scan->time_increment); 23 | ROS_INFO("[EAI INFO]: I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count); 24 | ROS_INFO("[EAI INFO]: angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max)); 25 | 26 | for(int i = 0; i < count; i++) { 27 | float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); 28 | ROS_INFO(": [%f, %f]", degree, scan->ranges[i]); 29 | } 30 | } 31 | 32 | int main(int argc, char **argv) 33 | { 34 | ros::init(argc, argv, "flash_lidar_client"); 35 | ros::NodeHandle n; 36 | 37 | ros::Subscriber sub = n.subscribe("/scan", 1000, scanCallback); 38 | 39 | ros::spin(); 40 | 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /src/flashgo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FLASH LIDAR SYSTEM 3 | * Flash Lidar ROS Node Client 4 | * 5 | * Copyright 2015 - 2017 EAI TEAM 6 | * http://www.eaibot.com 7 | * 8 | */ 9 | 10 | #include "ros/ros.h" 11 | #include "sensor_msgs/LaserScan.h" 12 | 13 | #include "flashgo.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #define NODE_COUNTS 720 20 | #define EACH_ANGLE 0.5 21 | #define DELAY_SECONDS 26 22 | #define DEG2RAD(x) ((x)*M_PI/180.) 23 | 24 | static bool flag = true; 25 | 26 | void publish_scan(ros::Publisher *pub, node_info *nodes, size_t node_count, ros::Time start, double scan_time, float angle_min, float angle_max, std::string frame_id, std::vector ignore_array) 27 | { 28 | sensor_msgs::LaserScan scan_msg; 29 | float nodes_array[node_count]; 30 | float quality_array[node_count]; 31 | for (size_t i = 0; i < node_count; i++) { 32 | if(i> 2); 35 | }else{ 36 | nodes_array[node_count-1-(i-node_count/2)] = (float)nodes[i].distance_q2/4.0f/1000; 37 | quality_array[node_count-1-(i-node_count/2)] = (float)(nodes[i].sync_quality >> 2); 38 | } 39 | 40 | if(ignore_array.size() != 0){ 41 | float angle = (float)((nodes[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); 42 | if(angle>180){ 43 | angle=360-angle; 44 | }else{ 45 | angle=-angle; 46 | } 47 | for(unsigned int j = 0; j < ignore_array.size();j = j+2){ 48 | if((ignore_array[j] < angle) && (angle <= ignore_array[j+1])){ 49 | if(ipublish(scan_msg); 86 | } 87 | 88 | 89 | std::vector split(const std::string &s, char delim) { 90 | std::vector elems; 91 | std::stringstream ss(s); 92 | std::string number; 93 | while(std::getline(ss, number, delim)) { 94 | elems.push_back(atoi(number.c_str())); 95 | } 96 | return elems; 97 | } 98 | 99 | static void Stop(int signo) 100 | { 101 | 102 | printf("Received exit signal\n"); 103 | flag = false; 104 | Flashgo::singleton()->disconnect(); 105 | Flashgo::done(); 106 | exit(1); 107 | 108 | } 109 | 110 | int main(int argc, char * argv[]) { 111 | ros::init(argc, argv, "flash_lidar_node"); 112 | 113 | std::string port; 114 | int baudrate; 115 | std::string frame_id; 116 | bool angle_fixed; 117 | double angle_max,angle_min; 118 | u_int32_t op_result; 119 | 120 | std::string list; 121 | std::vector ignore_array; 122 | 123 | ros::NodeHandle nh; 124 | ros::Publisher scan_pub = nh.advertise("scan", 1000); 125 | ros::NodeHandle nh_private("~"); 126 | nh_private.param("port", port, "/dev/ttyACM0"); 127 | nh_private.param("baudrate", baudrate, 115200); 128 | nh_private.param("frame_id", frame_id, "laser_frame"); 129 | nh_private.param("angle_fixed", angle_fixed, "true"); 130 | nh_private.param("angle_max", angle_max , 180); 131 | nh_private.param("angle_min", angle_min , -180); 132 | nh_private.param("ignore_array",list,""); 133 | 134 | ignore_array = split(list ,','); 135 | if(ignore_array.size()%2){ 136 | ROS_ERROR_STREAM("ignore array is odd need be even"); 137 | } 138 | 139 | for(unsigned int i =0 ; i < ignore_array.size();i++){ 140 | if(ignore_array[i] < -180 && ignore_array[i] > 180){ 141 | ROS_ERROR_STREAM("ignore array should be -180<= <=180"); 142 | } 143 | } 144 | 145 | 146 | Flashgo::initDriver(); 147 | if (!Flashgo::singleton()) { 148 | fprintf(stderr, "[EAI ERROR]: Create Driver fail, exit\n"); 149 | return -2; 150 | } 151 | signal(SIGINT, Stop); 152 | signal(SIGTERM, Stop); 153 | 154 | op_result = Flashgo::singleton()->connect(port.c_str(), (u_int32_t)baudrate); 155 | if (op_result == -1) { 156 | int seconds=0; 157 | while((op_result == -1) && (seconds <= DELAY_SECONDS) && flag){ 158 | sleep(2); 159 | seconds = seconds + 2; 160 | Flashgo::singleton()->disconnect(); 161 | op_result = Flashgo::singleton()->connect(port.c_str(), (u_int32_t)baudrate); 162 | fprintf(stdout, "[EAI INFO]: Try to connect the port %s again after %d s .\n", port.c_str() , seconds); 163 | if(op_result==0){ 164 | break; 165 | } 166 | } 167 | 168 | if(seconds > DELAY_SECONDS){ 169 | fprintf(stderr, "[EAI ERROR]: Cannot bind to the specified serial port %s.\n" , port.c_str()); 170 | Flashgo::singleton()->disconnect(); 171 | Flashgo::done(); 172 | return -1; 173 | } 174 | } 175 | 176 | fprintf(stdout, "[EAI INFO]: Connected the port %s , start to scan ......\n", port.c_str()); 177 | int ans=Flashgo::singleton()->startScan(); 178 | if(ans != 0){ 179 | ans = Flashgo::singleton()->startScan(); 180 | if(ans !=0){ 181 | fprintf(stdout, "Start LIDAR is failed! Exit!! ......\n"); 182 | Flashgo::singleton()->disconnect(); 183 | Flashgo::done(); 184 | } 185 | } 186 | fprintf(stdout, "[EAI INFO]: Now LIDAR is scanning ......\n"); 187 | flag = false; 188 | 189 | ros::Time start_scan_time; 190 | ros::Time end_scan_time; 191 | double scan_duration; 192 | ros::Rate rate(30); 193 | 194 | node_info all_nodes[NODE_COUNTS]; 195 | memset(all_nodes, 0, NODE_COUNTS*sizeof(node_info)); 196 | 197 | while (ros::ok()) { 198 | try{ 199 | node_info nodes[NODE_COUNTS]; 200 | size_t count = _countof(nodes); 201 | 202 | start_scan_time = ros::Time::now(); 203 | op_result = Flashgo::singleton()->grabScanData(nodes, count); 204 | 205 | end_scan_time = ros::Time::now(); 206 | scan_duration = (end_scan_time - start_scan_time).toSec(); 207 | 208 | if (op_result == 0) { 209 | op_result = Flashgo::singleton()->ascendScanData(nodes, count); 210 | 211 | if (op_result == 0) { 212 | if (angle_fixed) { 213 | memset(all_nodes, 0, NODE_COUNTS*sizeof(node_info)); 214 | int i = 0 ; 215 | for( ; i < count; i++) { 216 | if (nodes[i].distance_q2 != 0) { 217 | float angle = (float)((nodes[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); 218 | int inter =(int)( angle / EACH_ANGLE ); 219 | float angle_pre = angle - inter * EACH_ANGLE; 220 | float angle_next = (inter+1) * EACH_ANGLE - angle; 221 | if(angle_pre < angle_next){ 222 | all_nodes[inter]=nodes[i]; 223 | }else{ 224 | all_nodes[inter+1]=nodes[i]; 225 | } 226 | } 227 | } 228 | publish_scan(&scan_pub, all_nodes, NODE_COUNTS, start_scan_time, scan_duration, angle_min, angle_max, frame_id, ignore_array); 229 | } else { 230 | int start_node = 0, end_node = 0; 231 | int i = 0; 232 | while (nodes[i++].distance_q2 == 0); 233 | start_node = i-1; 234 | i = count -1; 235 | while (nodes[i--].distance_q2 == 0); 236 | end_node = i+1; 237 | 238 | angle_min = (float)(nodes[start_node].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; 239 | angle_max = (float)(nodes[end_node].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; 240 | 241 | publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1, start_scan_time, scan_duration, angle_min, angle_max, frame_id, ignore_array); 242 | } 243 | } 244 | }else if(op_result == -2){ 245 | // All the data is invalid, just publish them 246 | publish_scan(&scan_pub, all_nodes, NODE_COUNTS, start_scan_time, scan_duration,angle_min, angle_max,frame_id,ignore_array); 247 | } 248 | 249 | rate.sleep(); 250 | ros::spinOnce(); 251 | }catch(std::exception &e){// 252 | std::cerr << "Unhandled Exception: " << e.what() << std::endl; 253 | Flashgo::singleton()->disconnect(); 254 | printf("[EAI INFO]: Now LIDAR is stopping .......\n"); 255 | Flashgo::done(); 256 | return 0; 257 | }catch(...){//anthor exception 258 | std::cerr << "Unhandled Exception:Unknown " <disconnect(); 260 | printf("[EAI INFO]: Now LIDAR is stopping .......\n"); 261 | Flashgo::done(); 262 | return 0; 263 | } 264 | } 265 | 266 | Flashgo::singleton()->disconnect(); 267 | printf("[EAI INFO]: Now LIDAR is stopping .......\n"); 268 | Flashgo::done(); 269 | return 0; 270 | } 271 | -------------------------------------------------------------------------------- /src/locker.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #pragma once 4 | 5 | 6 | class Locker 7 | { 8 | public: 9 | enum LOCK_STATUS 10 | { 11 | LOCK_OK = 1, 12 | LOCK_TIMEOUT = -1, 13 | LOCK_FAILED = 0 14 | }; 15 | 16 | Locker(){ 17 | #ifdef _WIN32 18 | _lock = NULL; 19 | #endif 20 | init(); 21 | } 22 | 23 | ~Locker() 24 | { 25 | release(); 26 | } 27 | 28 | Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF) 29 | { 30 | #ifdef _WIN32 31 | switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) 32 | { 33 | case WAIT_ABANDONED: 34 | return LOCK_FAILED; 35 | case WAIT_OBJECT_0: 36 | return LOCK_OK; 37 | case WAIT_TIMEOUT: 38 | return LOCK_TIMEOUT; 39 | } 40 | 41 | #else 42 | #ifdef _MACOS 43 | if (timeout !=0 ) { 44 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 45 | } 46 | #else 47 | if (timeout == 0xFFFFFFFF){ 48 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 49 | } 50 | #endif 51 | else if (timeout == 0) 52 | { 53 | if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK; 54 | } 55 | #ifndef _MACOS 56 | else 57 | { 58 | timespec wait_time; 59 | timeval now; 60 | gettimeofday(&now,NULL); 61 | 62 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 63 | wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; 64 | 65 | if (wait_time.tv_nsec >= 1000000000) 66 | { 67 | ++wait_time.tv_sec; 68 | wait_time.tv_nsec -= 1000000000; 69 | } 70 | switch (pthread_mutex_timedlock(&_lock,&wait_time)) 71 | { 72 | case 0: 73 | return LOCK_OK; 74 | case ETIMEDOUT: 75 | return LOCK_TIMEOUT; 76 | } 77 | } 78 | #endif 79 | #endif 80 | 81 | return LOCK_FAILED; 82 | } 83 | 84 | 85 | void unlock() 86 | { 87 | #ifdef _WIN32 88 | ReleaseMutex(_lock); 89 | #else 90 | pthread_mutex_unlock(&_lock); 91 | #endif 92 | } 93 | 94 | #ifdef _WIN32 95 | HANDLE getLockHandle() 96 | { 97 | return _lock; 98 | } 99 | #else 100 | pthread_mutex_t *getLockHandle() 101 | { 102 | return &_lock; 103 | } 104 | #endif 105 | 106 | 107 | 108 | protected: 109 | void init() 110 | { 111 | #ifdef _WIN32 112 | _lock = CreateMutex(NULL,FALSE,NULL); 113 | #else 114 | pthread_mutex_init(&_lock, NULL); 115 | #endif 116 | } 117 | 118 | void release() 119 | { 120 | unlock(); 121 | #ifdef _WIN32 122 | 123 | if (_lock) CloseHandle(_lock); 124 | _lock = NULL; 125 | #else 126 | pthread_mutex_destroy(&_lock); 127 | #endif 128 | } 129 | 130 | #ifdef _WIN32 131 | HANDLE _lock; 132 | #else 133 | pthread_mutex_t _lock; 134 | #endif 135 | }; 136 | 137 | class ScopedLocker 138 | { 139 | public : 140 | ScopedLocker(Locker &l): _binded(l) 141 | { 142 | _binded.lock(); 143 | } 144 | 145 | void forceUnlock() { 146 | _binded.unlock(); 147 | } 148 | ~ScopedLocker() {_binded.unlock();} 149 | Locker & _binded; 150 | }; 151 | 152 | 153 | -------------------------------------------------------------------------------- /startup/initenv.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0042", MODE:="0666", GROUP:="dialout", SYMLINK+="dashgo"' >/etc/udev/rules.d/dashgo.rules 3 | 4 | echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0666", GROUP:="dialout", SYMLINK+="dashgo"' >/etc/udev/rules.d/ch34x.rules 5 | 6 | echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout", SYMLINK+="flashlidar"' >/etc/udev/rules.d/flashlidar.rules 7 | 8 | echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout", SYMLINK+="flashlidar"' >/etc/udev/rules.d/flashlidar-V2.rules 9 | 10 | echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="ttyS2"' >/etc/udev/rules.d/imu.rules 11 | 12 | service udev reload 13 | sleep 2 14 | service udev restart 15 | 16 | --------------------------------------------------------------------------------