From ca7b81a506368c56b58e686c487a795ee914c3ef Mon Sep 17 00:00:00 2001 From: EAIBOT Date: Fri, 4 Nov 2016 16:01:57 +0800 Subject: [PATCH 1/4] Add files via upload --- CMakeLists.txt | 198 +++++++++ README.md | 31 +- launch/lidar.launch | 9 + launch/lidar.rviz | 128 ++++++ launch/view_lidar.launch | 10 + package.xml | 56 +++ src/flashgo.cpp | 936 +++++++++++++++++++++++++++++++++++++++ src/flashgo.h | 165 +++++++ src/flashgo_.cpp | 917 ++++++++++++++++++++++++++++++++++++++ src/flashgo_.h | 164 +++++++ src/flashgo_client.cpp | 39 ++ src/flashgo_node.cpp | 256 +++++++++++ 12 files changed, 2907 insertions(+), 2 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 launch/lidar.launch create mode 100644 launch/lidar.rviz create mode 100644 launch/view_lidar.launch create mode 100644 package.xml create mode 100644 src/flashgo.cpp create mode 100644 src/flashgo.h create mode 100644 src/flashgo_.cpp create mode 100644 src/flashgo_.h create mode 100644 src/flashgo_client.cpp create mode 100644 src/flashgo_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..901c1ce --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,198 @@ +cmake_minimum_required(VERSION 2.8.3) +project(flashgo) + +## 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 + rosconsole + roscpp + sensor_msgs +) + +## 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 run_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 run_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 +# sensor_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 run_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 you 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 flashgo +# CATKIN_DEPENDS rosconsole roscpp sensor_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS}/src +) + +## Declare a C++ library +# add_library(flashgo +# src/${PROJECT_NAME}/flashgo.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(flashgo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(flashgo_node src/flashgo_node.cpp) + +add_executable(flashgo_node src/flashgo_node.cpp src/flashgo.cpp) +add_executable(flashgo_client src/flashgo_client.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(flashgo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(flashgo_node +# ${catkin_LIBRARIES} +# ) + +target_link_libraries(flashgo_node + ${catkin_LIBRARIES} + ) +target_link_libraries(flashgo_client + ${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 flashgo flashgo_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_flashgo.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) diff --git a/README.md b/README.md index e2605d1..dbcefcb 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,29 @@ -# flashgo -The driver of Flash Lidar for ROS +FLASH LIDAR ROS package +===================================================================== + +ROS node and test application for FLASH LIDAR + +Visit EAI Website for more details about FLASH LIDAR. + +How to build FLASH LIDAR ros package +===================================================================== + 1) Clone this project to your catkin's workspace src folder + 2) Running catkin_make to build flashgo_node and flashgo_client + +How to run FLASH LIDAR ros package +===================================================================== +There're two ways to run FLASH LIDAR ros package + +1. Run FLASH LIDAR node and view in the rviz +------------------------------------------------------------ +roslaunch flashgo view_lidar.launch + +You should see FLASH LIDAR's scan result in the rviz. + +2. Run FLASH LIDAR node and view using test application +------------------------------------------------------------ +roslaunch flashgo lidar.launch + +rosrun flashgo flashgo_client + +You should see FLASH LIDAR's scan result in the console diff --git a/launch/lidar.launch b/launch/lidar.launch new file mode 100644 index 0000000..ccbdae8 --- /dev/null +++ b/launch/lidar.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/lidar.rviz b/launch/lidar.rviz new file mode 100644 index 0000000..22df835 --- /dev/null +++ b/launch/lidar.rviz @@ -0,0 +1,128 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /FlashLidarLaserScan1 + Splitter Ratio: 0.5 + Tree Height: 413 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0 + Min Value: 0 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: FlashLidarLaserScan + Position Transformer: XYZ + Queue Size: 1000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.03 + Style: Squares + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: laser + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 11.1184 + Focal Point: + X: -0.0344972 + Y: 0.065886 + Z: 0.148092 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.615399 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.82358 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 626 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001950000022cfc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000022c000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003240000022c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1215 + X: 65 + Y: 24 diff --git a/launch/view_lidar.launch b/launch/view_lidar.launch new file mode 100644 index 0000000..f36f624 --- /dev/null +++ b/launch/view_lidar.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..4396554 --- /dev/null +++ b/package.xml @@ -0,0 +1,56 @@ + + + flashgo + 1.0.0 + The flashgo package + + + + + shao + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rosconsole + roscpp + sensor_msgs + rosconsole + roscpp + sensor_msgs + + + + + + + + diff --git a/src/flashgo.cpp b/src/flashgo.cpp new file mode 100644 index 0000000..4e2b744 --- /dev/null +++ b/src/flashgo.cpp @@ -0,0 +1,936 @@ +#include "flashgo.h" + +static int serial_fd; +static pthread_t threadId; +size_t required_tx_cnt; +size_t required_rx_cnt; +u_int32_t _baudrate; + +Flashgo::Flashgo() +{ + isConnected = false; + isScanning = false; + scan_data_protocol = 0; + pthread_mutex_init(&_lock, NULL); +} + +int Flashgo::connect(const char * port_path, u_int32_t baudrate) +{ + _baudrate = baudrate; + if (isConnected){ + return 1; + } + + { + if (serial_fd != -1) { + close(serial_fd); + } + + serial_fd = -1; + serial_fd = open(port_path, O_RDWR | O_NOCTTY | O_NDELAY); + if (serial_fd == -1) { + return -1; + } + + struct termios options, oldopt; + tcgetattr(serial_fd, &oldopt); + bzero(&options,sizeof(struct termios)); + + u_int32_t termbaud = getTermBaudBitmap(baudrate); + if (termbaud == (u_int32_t)-1) { + if (serial_fd != -1) { + close(serial_fd); + } + serial_fd = -1; + return -1; + } + cfsetispeed(&options, termbaud); + cfsetospeed(&options, termbaud); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + options.c_iflag &= ~(IXON | IXOFF | IXANY); + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_oflag &= ~OPOST; + + tcflush(serial_fd,TCIFLUSH); + if (fcntl(serial_fd, F_SETFL, FNDELAY)) { + if (serial_fd != -1) { + close(serial_fd); + } + serial_fd = -1; + return -1; + } + if (tcsetattr(serial_fd, TCSANOW, &options)) { + if (serial_fd != -1) { + close(serial_fd); + } + serial_fd = -1; + return -1; + } + + u_int32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIC, &dtr_bit); + } + + isConnected = true; + return serial_fd; +} + + +void Flashgo::disconnect() +{ + if (!isConnected){ + return ; + } + stop(); + if (serial_fd != -1) { + close(serial_fd); + } + isConnected = false; + serial_fd = -1; +} + +u_int32_t Flashgo::getTermBaudBitmap(u_int32_t baud) +{ + #define BAUD_CONV( _baud_) case _baud_: return B##_baud_ + switch (baud) { + BAUD_CONV(1200); + BAUD_CONV(1800); + BAUD_CONV(2400); + BAUD_CONV(4800); + BAUD_CONV(9600); + BAUD_CONV(19200); + BAUD_CONV(38400); + BAUD_CONV(57600); + BAUD_CONV(115200); + BAUD_CONV(230400); + BAUD_CONV(460800); + BAUD_CONV(500000); + BAUD_CONV(576000); + BAUD_CONV(921600); + BAUD_CONV(1000000); + BAUD_CONV(1152000); + BAUD_CONV(1500000); + BAUD_CONV(2000000); + BAUD_CONV(2500000); + BAUD_CONV(3000000); + BAUD_CONV(3500000); + BAUD_CONV(4000000); + } + return -1; +} + +void Flashgo::disableDataGrabbing() +{ + isScanning = false; + pthread_join(threadId , NULL); +} + +int Flashgo::sendCommand(u_int8_t cmd, const void * payload, size_t payloadsize) +{ + u_int8_t pkt_header[10]; + cmd_packet * header = reinterpret_cast(pkt_header); + u_int8_t checksum = 0; + + if (!isConnected) { + return -2; + } + if (payloadsize && payload) { + cmd |= LIDAR_CMDFLAG_HAS_PAYLOAD; + } + + header->syncByte = LIDAR_CMD_SYNC_BYTE; + header->cmd_flag = cmd; + sendData(pkt_header, 2) ; + + if (cmd & LIDAR_CMDFLAG_HAS_PAYLOAD) { + checksum ^= LIDAR_CMD_SYNC_BYTE; + checksum ^= cmd; + checksum ^= (payloadsize & 0xFF); + + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum ^= ((u_int8_t *)payload)[pos]; + } + + u_int8_t sizebyte = payloadsize; + sendData(&sizebyte, 1); + + sendData((const u_int8_t *)payload, sizebyte); + + sendData(&checksum, 1); + } + return 0; +} + +int Flashgo::sendData(const unsigned char * data, size_t size) +{ + if (!isConnected) { + return 0; + } + + if (data == NULL || size ==0) { + return 0; + } + + size_t tx_len = 0; + required_tx_cnt = 0; + do { + int ans = write(serial_fd, data + tx_len, size-tx_len); + if (ans == -1) { + return tx_len; + } + tx_len += ans; + required_tx_cnt = tx_len; + }while (tx_len(header); + u_int32_t waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(lidar_ans_header) - recvPos; + size_t recvSize; + + int ans = waitForData(remainSize, timeout - waitTime, &recvSize); + if (ans == -2){ + return -2; + }else if (ans == -1){ + return -1; + } + + if(recvSize > remainSize) recvSize = remainSize; + + ans = getData(recvBuffer, recvSize); + if (ans == -1){ + return -2; + } + + for (size_t pos = 0; pos < recvSize; ++pos) { + u_int8_t currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + continue; + } + break; + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE2) { + recvPos = 0; + continue; + } + break; + } + headerBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(lidar_ans_header)) { + return 0; + } + } + } + return -1; +} + +int Flashgo::waitForData(size_t data_count, u_int32_t timeout, size_t * returned_size) +{ + size_t length = 0; + if (returned_size==NULL) { + returned_size=(size_t *)&length; + } + + int max_fd; + fd_set input_set; + struct timeval timeout_val; + + FD_ZERO(&input_set); + FD_SET(serial_fd, &input_set); + max_fd = serial_fd + 1; + + timeout_val.tv_sec = timeout / 1000; + timeout_val.tv_usec = (timeout % 1000) * 1000; + + if ( isConnected ){ + if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { + return -2; + } + if (*returned_size >= data_count){ + return 0; + } + } + + while (isConnected) { + int n = select(max_fd, &input_set, NULL, NULL, &timeout_val); + if (n < 0){ + return -2; + }else if (n == 0) { + return -1; + } else { + assert (FD_ISSET(serial_fd, &input_set)); + if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { + return -2; + } + + if (*returned_size >= data_count){ + return 0; + } else { + int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec; + int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate; + if (remain_timeout > expect_remain_time){ + usleep(expect_remain_time); + } + } + } + } + return -2; +} + +int Flashgo::getHealth(device_health & health, u_int32_t timeout) +{ + int ans; + if (!isConnected) { + return -2; + } + + disableDataGrabbing(); + { + if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_HEALTH)) != 0) { + return ans; + } + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVHEALTH) { + return -3; + } + + if (response_header.size < sizeof(device_health)) { + return -3; + } + + if (waitForData(response_header.size, timeout) != 0) { + return -1; + } + + getData(reinterpret_cast(&health), sizeof(health)); + } + return 0; +} + + +int Flashgo::getDeviceInfo(device_info & info, u_int32_t timeout) +{ + int ans; + if (!isConnected) { + return -2; + } + + disableDataGrabbing(); + { + if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_INFO)) != 0) { + return ans; + } + + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) { + return -3; + } + + if (response_header.size < sizeof(lidar_ans_header)) { + return -3; + } + + if (waitForData(response_header.size, timeout) != 0) { + return -1; + } + getData(reinterpret_cast(&info), sizeof(info)); + } + return 0; +} + +int Flashgo::startScan(bool force, u_int32_t timeout ) +{ + int ans; + if (!isConnected) { + return -2; + } + if (isScanning) { + return 0; + } + + stop(); + { + if ((ans = sendCommand(force?LIDAR_CMD_FORCE_SCAN:LIDAR_CMD_SCAN)) != 0) { + return ans; + } + + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_MEASUREMENT) { + return -3; + } + + if (response_header.size < sizeof(node_info)) { + return -3; + } + + isScanning = true; + ans = this->createThread(); + return ans; + } + return 0; +} + +void * Flashgo::_thread_t(void *args) +{ + Flashgo* pThis = static_cast(args); + pThis->cacheScanData(); + return NULL; +} + +int Flashgo::createThread() +{ + if(pthread_create(&threadId,NULL,_thread_t,(void *)this) != 0) { + return -2; + } + return 0; +} + +int Flashgo::stop() +{ + int ans; + node_info local_buf[128]; + size_t count = 128; + + disableDataGrabbing(); + sendCommand(LIDAR_CMD_FORCE_STOP); + + while(true) { + if ((ans = waitScanData(local_buf, count ,10)) != 0 ) { + if (ans == -1) { + break; + } + }else{ + sendCommand(LIDAR_CMD_FORCE_STOP); + } + } + sendCommand(LIDAR_CMD_STOP); + return 0; +} + +int Flashgo::cacheScanData() +{ + node_info local_buf[128]; + size_t count = 128; + node_info local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + int ans; + memset(local_scan, 0, sizeof(local_scan)); + + waitScanData(local_buf, count); + + while(isScanning) { + if ((ans=waitScanData(local_buf, count)) != 0) { + if (ans != -1) { + isScanning = false; + return -2; + } + } + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT) { + if ((local_scan[0].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + this->lock(); + memcpy(scan_node_buf, local_scan, scan_count*sizeof(node_info)); + scan_node_count = scan_count; + pthread_mutex_unlock(&_lock); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == sizeof(local_scan)) scan_count-=1; + } + } + isScanning = false; + return 0; +} + +int Flashgo::waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout) +{ + if (!isConnected) { + count = 0; + return -2; + } + + size_t recvNodeCount = 0; + u_int32_t startTs = getms(); + u_int32_t waitTime; + int ans; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { + node_info node; + if(scan_data_protocol == 0){ + if ((ans = waitNode(&node, timeout - waitTime)) != 0) { + return ans; + } + }else{ + if ((ans = waitPackage(&node, timeout - waitTime)) != 0) { + return ans; + } + } + + nodebuffer[recvNodeCount++] = node; + if (recvNodeCount == count) { + return 0; + } + } + count = recvNodeCount; + return -1; +} + +int Flashgo::waitNode(node_info * node, u_int32_t timeout) +{ + int recvPos = 0; + u_int32_t startTs = getms(); + u_int8_t recvBuffer[sizeof(node_info)]; + u_int8_t *nodeBuffer = (u_int8_t*)node; + u_int32_t waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(node_info) - recvPos; + size_t recvSize; + + int ans = waitForData(remainSize, timeout-waitTime, &recvSize); + if (ans == -2) { + return -2; + }else if (ans == -1){ + return -1; + } + + if (recvSize > remainSize) recvSize = remainSize; + + getData(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + u_int8_t currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + { + u_int8_t tmp = (currentByte>>1); + if ( (tmp ^ currentByte) & 0x1 ) { + + } else { + continue; + } + + } + break; + case 1: + { + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + + } else { + recvPos = 0; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(node_info)) { + return 0; + } + } + } + + return -1; +} + +int Flashgo::waitPackage(node_info * node, u_int32_t timeout) +{ + int recvPos = 0; + int recvPosEnd = 0; + u_int32_t startTs = getms(); + u_int8_t recvBuffer[sizeof(node_package)]; + u_int8_t *nodeBuffer = (u_int8_t*)node; + u_int32_t waitTime; + + static node_package package; + static u_int16_t package_Sample_Index = 0; + static u_int16_t IntervalSampleAngle = 0; + static u_int16_t IntervalSampleAngle_LastPackage = 0; + static u_int16_t FirstSampleAngle = 0; + static u_int16_t LastSampleAngle = 0; + + u_int8_t *packageBuffer = (u_int8_t*)&package.package_Head; + u_int8_t package_Sample_Num = 0; + + int package_recvPos = 0; + + if(package_Sample_Index == 0) { + recvPos = 0; + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = PackagePaidBytes - recvPos; + size_t recvSize; + int ans = waitForData(remainSize, timeout-waitTime, &recvSize); + if (ans == -2){ + return -2; + }else if (ans == -1){ + return -1; + } + + if (recvSize > remainSize) recvSize = remainSize; + + getData(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + u_int8_t currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + { + if ( currentByte == (PH&0xFF) ) { + + } else { + continue; + } + + } + break; + case 1: + { + if ( currentByte == (PH>>8) ) { + + } else { + recvPos = 0; + continue; + } + } + break; + case 2: + { + if ((currentByte == CT_Normal) || (currentByte == CT_RingStart)){ + + } else { + recvPos = 0; + continue; + } + } + break; + case 3: + package_Sample_Num = currentByte; + break; + case 4: + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + FirstSampleAngle = currentByte; + } else { + recvPos = 0; + continue; + } + break; + case 5: + FirstSampleAngle = currentByte*0x100 + FirstSampleAngle; + FirstSampleAngle = FirstSampleAngle>>1; + break; + case 6: + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + LastSampleAngle = currentByte; + } else { + recvPos = 0; + continue; + } + break; + case 7: + LastSampleAngle = currentByte*0x100 + LastSampleAngle; + LastSampleAngle = LastSampleAngle>>1; + if(package_Sample_Num == 1){ + IntervalSampleAngle = 0; + }else{ + if(LastSampleAngle < FirstSampleAngle){ + if((FirstSampleAngle > 270*64) && (LastSampleAngle < 90*64)){ + IntervalSampleAngle = (360*64 + LastSampleAngle - FirstSampleAngle)/(package_Sample_Num-1); + IntervalSampleAngle_LastPackage = IntervalSampleAngle; + } else{ + IntervalSampleAngle = IntervalSampleAngle_LastPackage; + } + } else{ + IntervalSampleAngle = (LastSampleAngle -FirstSampleAngle)/(package_Sample_Num-1); + IntervalSampleAngle_LastPackage = IntervalSampleAngle; + } + } + break; + } + packageBuffer[recvPos++] = currentByte; + } + + if (recvPos == PackagePaidBytes ){ + package_recvPos = recvPos; + break; + } + } + + if(PackagePaidBytes == recvPos){ + startTs = getms(); + recvPos = 0; + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = package_Sample_Num*PackageSampleBytes - recvPos; + size_t recvSize; + int ans =waitForData(remainSize, timeout-waitTime, &recvSize); + if (ans == -2){ + return -2; + }else if (ans == -1){ + return -1; + } + + if (recvSize > remainSize) recvSize = remainSize; + + getData(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + packageBuffer[package_recvPos+recvPos] = recvBuffer[pos]; + recvPos++; + } + if(package_Sample_Num*PackageSampleBytes == recvPos){ + package_recvPos += recvPos; + break; + } + } + if(package_Sample_Num*PackageSampleBytes != recvPos){ + return -1; + } + } else { + return -1; + } + } + + if(package.package_CT == CT_Normal){ + (*node).sync_quality = Node_Default_Quality + Node_NotSync; + } else{ + (*node).sync_quality = Node_Default_Quality + Node_Sync; + } + + (*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT; + (*node).distance_q2 = package.packageSampleDistance[package_Sample_Index]; + + package_Sample_Index++; + if(package_Sample_Index >= package.nowPackageNum){ + package_Sample_Index = 0; + } + return 0; +} + + +int Flashgo::grabScanData(node_info * nodebuffer, size_t & count) +{ + { + if(scan_node_count == 0) { + return -2; + } + size_t size_to_copy = min(count, scan_node_count); + memcpy(nodebuffer, scan_node_buf, size_to_copy*sizeof(node_info)); + count = size_to_copy; + scan_node_count = 0; + } + return 0; +} + +void Flashgo::simpleScanData(std::vector *scan_data , node_info *buffer, size_t count) +{ + scan_data->clear(); + for (int pos = 0; pos < (int)count; ++pos) { + scanDot dot; + if (!buffer[pos].distance_q2) continue; + dot.quality = (buffer[pos].sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + dot.angle = (buffer[pos].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + dot.dist = buffer[pos].distance_q2/4.0f; + scan_data->push_back(dot); + } +} + +int Flashgo::ascendScanData(node_info * nodebuffer, size_t count) +{ + float inc_origin_angle = 360.0/count; + node_info *tmpbuffer = new node_info[count]; + int i = 0; + + for (i = 0; i < (int)count; i++) { + if(nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while(i != 0) { + i--; + float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle; + if (expect_angle < 0.0f) expect_angle = 0.0f; + u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + break; + } + } + + if (i == (int)count) return -3; + + for (i = (int)count - 1; i >= 0; i--) { + if(nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while(i != ((int)count - 1)) { + i++; + float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + break; + } + } + + float frontAngle = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + for (i = 1; i < (int)count; i++) { + if(nodebuffer[i].distance_q2 == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + } + + size_t zero_pos = 0; + float pre_degree = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + + for (i = 1; i < (int)count ; ++i) { + float degree = (nodebuffer[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + if (zero_pos == 0 && (pre_degree - degree > 180)) { + zero_pos = i; + break; + } + pre_degree = degree; + } + + for (i = (int)zero_pos; i < (int)count; i++) { + tmpbuffer[i-zero_pos] = nodebuffer[i]; + } + for (i = 0; i < (int)zero_pos; i++) { + tmpbuffer[i+(int)count-zero_pos] = nodebuffer[i]; + } + + memcpy(nodebuffer, tmpbuffer, count*sizeof(node_info)); + delete tmpbuffer; + + return 0; +} + +int Flashgo::lock(unsigned long timeout) +{ + if (timeout == 0xFFFFFFFF){ + if (pthread_mutex_lock(&_lock) == 0) return 1; + } else if (timeout == 0) { + if (pthread_mutex_trylock(&_lock) == 0) return 1; + } else { + struct timespec wait_time; + timeval now; + gettimeofday(&now,NULL); + + wait_time.tv_sec = timeout/1000 + now.tv_sec; + wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; + + if (wait_time.tv_nsec >= 1000000000) { + ++wait_time.tv_sec; + wait_time.tv_nsec -= 1000000000; + } + switch (pthread_mutex_timedlock(&_lock,&wait_time)) { + case 0: + return 1; + case ETIMEDOUT: + return -1; + } + } + return 0; +} + +void Flashgo::releaseThreadLock() +{ + pthread_mutex_unlock(&_lock); + pthread_mutex_destroy(&_lock); +} + +void Flashgo::setScanDataProtocol(int protocol) +{ + scan_data_protocol = protocol; +} + +int Flashgo::getEAI( u_int32_t timeout) +{ + int ans; + + if (!isConnected) return -2; + + disableDataGrabbing(); + + { + if ((ans = sendCommand(LIDAR_CMD_GET_EAI)) != 0) { + return ans; + } + + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { + return ans; + } + + if (response_header.size < 3) { + return -3; + } + + if (waitForData(response_header.size, timeout) != 0) { + return -1; + } + + char eaiInfo[3]={0}; + getData(reinterpret_cast(&eaiInfo), sizeof(eaiInfo)); + + //fprintf(stderr, " eaiInfo : %s \n" ,eaiInfo); + if(eaiInfo[0]=='E' && eaiInfo[1]=='A' && eaiInfo[2]=='I'){ + return 0; + } + + } + return -2; +} diff --git a/src/flashgo.h b/src/flashgo.h new file mode 100644 index 0000000..7485bbe --- /dev/null +++ b/src/flashgo.h @@ -0,0 +1,165 @@ +#ifndef FLASHGO_H +#define FLASHGO_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#define LIDAR_CMD_STOP 0x25 +#define LIDAR_CMD_SCAN 0x20 +#define LIDAR_CMD_FORCE_SCAN 0x21 +#define LIDAR_CMD_FORCE_STOP 0x00 +#define LIDAR_CMD_GET_EAI 0x55 +#define LIDAR_CMD_GET_DEVICE_INFO 0x50 +#define LIDAR_CMD_GET_DEVICE_HEALTH 0x52 +#define LIDAR_ANS_TYPE_DEVINFO 0x4 +#define LIDAR_ANS_TYPE_DEVHEALTH 0x6 +#define LIDAR_CMD_SYNC_BYTE 0xA5 +#define LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 +#define LIDAR_ANS_SYNC_BYTE1 0xA5 +#define LIDAR_ANS_SYNC_BYTE2 0x5A +#define LIDAR_ANS_TYPE_MEASUREMENT 0x81 +#define LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) +#define LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 +#define LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) +#define LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 + +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + + +#define PackageSampleMaxLngth 0x100 +typedef enum +{ + CT_Normal = 0, + CT_RingStart = 1, + CT_Tail, +}CT; +#define Node_Default_Quality (10<<2) +#define Node_Sync 1 +#define Node_NotSync 2 +#define PackagePaidBytes 8 +#define PackageSampleBytes 2 +#define PH 0x55AA + +struct node_info{ + u_int8_t sync_quality; + u_int16_t angle_q6_checkbit; + u_int16_t distance_q2; +} __attribute__((packed)) ; + +struct node_package { + u_int16_t package_Head; + u_int8_t package_CT; + u_int8_t nowPackageNum; + u_int16_t packageFirstSampleAngle; + u_int16_t packageLastSampleAngle; + u_int16_t packageSampleDistance[PackageSampleMaxLngth]; +} __attribute__((packed)) ; + +struct device_info{ + u_int8_t model; + u_int16_t firmware_version; + u_int8_t hardware_version; + u_int8_t serialnum[16]; +} __attribute__((packed)) ; + + struct device_health { + u_int8_t status; + u_int16_t error_code; +} __attribute__((packed)) ; + + struct cmd_packet { + u_int8_t syncByte; + u_int8_t cmd_flag; + u_int8_t size; + u_int8_t data[0]; +} __attribute__((packed)) ; + + struct lidar_ans_header { + u_int8_t syncByte1; + u_int8_t syncByte2; + u_int32_t size:30; + u_int32_t subType:2; + u_int8_t type; +} __attribute__((packed)); + +struct scanDot { + u_int8_t quality; + float angle; + float dist; + }; + +class Flashgo +{ +public: + Flashgo(); + int connect(const char * port_path, u_int32_t baudrate); + void disconnect(); + int getHealth(device_health & health, u_int32_t timeout = DEFAULT_TIMEOUT); + int getDeviceInfo(device_info & info, u_int32_t timeout = DEFAULT_TIMEOUT); + int startScan(bool force = false, u_int32_t timeout = DEFAULT_TIMEOUT) ; + int stop(); + int grabScanData(node_info * nodebuffer, size_t & count) ; + int ascendScanData(node_info * nodebuffer, size_t count); + int createThread(); + u_int32_t getms(); + void simpleScanData(std::vector * scan_data , node_info *buffer, size_t count); + int lock(unsigned long timeout = 0xFFFFFFFF); + void releaseThreadLock(); + void setScanDataProtocol(int protocol); + int getEAI(u_int32_t timeout = DEFAULT_TIMEOUT); + +protected: + int waitNode(node_info * node, u_int32_t timeout = DEFAULT_TIMEOUT); + int waitPackage(node_info * node, u_int32_t timeout = DEFAULT_TIMEOUT); + int waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout = DEFAULT_TIMEOUT); + int cacheScanData(); + int sendCommand(u_int8_t cmd, const void * payload = NULL, size_t payloadsize = 0); + int waitResponseHeader(lidar_ans_header * header, u_int32_t timeout = DEFAULT_TIMEOUT); + u_int32_t getTermBaudBitmap(u_int32_t baud); + int waitForData(size_t data_count,u_int32_t timeout = -1, size_t * returned_size = NULL); + int getData(unsigned char * data, size_t size); + int sendData(const unsigned char * data, size_t size); + void disableDataGrabbing(); + static void* _thread_t(void* args); + +public: + bool isConnected; + bool isScanning; + enum { + DEFAULT_TIMEOUT = 2000, //2000 ms + MAX_SCAN_NODES = 2048, + }; + node_info scan_node_buf[2048]; + size_t scan_node_count; + pthread_mutex_t _lock; + + int scan_data_protocol ; +}; + +#endif // FLASHGO_H diff --git a/src/flashgo_.cpp b/src/flashgo_.cpp new file mode 100644 index 0000000..a1285b7 --- /dev/null +++ b/src/flashgo_.cpp @@ -0,0 +1,917 @@ +#include "flashgo.h" + +static int serial_fd; +static pthread_t threadId; +size_t required_tx_cnt; +size_t required_rx_cnt; +u_int32_t _baudrate; + +Flashgo::Flashgo() +{ + isConnected = false; + isScanning = false; + pthread_mutex_init(&_lock, NULL); +} + +int Flashgo::connect(const char * port_path, u_int32_t baudrate) +{ + _baudrate = baudrate; + if (isConnected){ + return 1; + } + + { + if (serial_fd != -1) { + close(serial_fd); + } + + serial_fd = -1; + serial_fd = open(port_path, O_RDWR | O_NOCTTY | O_NDELAY); + if (serial_fd == -1) { + return -1; + } + + struct termios options, oldopt; + tcgetattr(serial_fd, &oldopt); + bzero(&options,sizeof(struct termios)); + + u_int32_t termbaud = getTermBaudBitmap(baudrate); + if (termbaud == (u_int32_t)-1) { + if (serial_fd != -1) { + close(serial_fd); + } + serial_fd = -1; + return -1; + } + cfsetispeed(&options, termbaud); + cfsetospeed(&options, termbaud); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~PARENB; + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + options.c_iflag &= ~(IXON | IXOFF | IXANY); + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_oflag &= ~OPOST; + + tcflush(serial_fd,TCIFLUSH); + if (fcntl(serial_fd, F_SETFL, FNDELAY)) { + if (serial_fd != -1) { + close(serial_fd); + } + serial_fd = -1; + return -1; + } + if (tcsetattr(serial_fd, TCSANOW, &options)) { + if (serial_fd != -1) { + close(serial_fd); + } + serial_fd = -1; + return -1; + } + + u_int32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIC, &dtr_bit); + } + + isConnected = true; + return serial_fd; +} + + +void Flashgo::disconnect() +{ + if (!isConnected){ + return ; + } + stop(); + if (serial_fd != -1) { + close(serial_fd); + } + isConnected = false; + serial_fd = -1; +} + +u_int32_t Flashgo::getTermBaudBitmap(u_int32_t baud) +{ + #define BAUD_CONV( _baud_) case _baud_: return B##_baud_ + switch (baud) { + BAUD_CONV(1200); + BAUD_CONV(1800); + BAUD_CONV(2400); + BAUD_CONV(4800); + BAUD_CONV(9600); + BAUD_CONV(19200); + BAUD_CONV(38400); + BAUD_CONV(57600); + BAUD_CONV(115200); + BAUD_CONV(230400); + BAUD_CONV(460800); + BAUD_CONV(500000); + BAUD_CONV(576000); + BAUD_CONV(921600); + BAUD_CONV(1000000); + BAUD_CONV(1152000); + BAUD_CONV(1500000); + BAUD_CONV(2000000); + BAUD_CONV(2500000); + BAUD_CONV(3000000); + BAUD_CONV(3500000); + BAUD_CONV(4000000); + } + return -1; +} + +void Flashgo::disableDataGrabbing() +{ + isScanning = false; + pthread_join(threadId , NULL); +} + +int Flashgo::sendCommand(u_int8_t cmd, const void * payload, size_t payloadsize) +{ + u_int8_t pkt_header[10]; + cmd_packet * header = reinterpret_cast(pkt_header); + u_int8_t checksum = 0; + + if (!isConnected) { + return -2; + } + if (payloadsize && payload) { + cmd |= LIDAR_CMDFLAG_HAS_PAYLOAD; + } + + header->syncByte = LIDAR_CMD_SYNC_BYTE; + header->cmd_flag = cmd; + sendData(pkt_header, 2) ; + + if (cmd & LIDAR_CMDFLAG_HAS_PAYLOAD) { + checksum ^= LIDAR_CMD_SYNC_BYTE; + checksum ^= cmd; + checksum ^= (payloadsize & 0xFF); + + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum ^= ((u_int8_t *)payload)[pos]; + } + + u_int8_t sizebyte = payloadsize; + sendData(&sizebyte, 1); + + sendData((const u_int8_t *)payload, sizebyte); + + sendData(&checksum, 1); + } + return 0; +} + +int Flashgo::sendData(const unsigned char * data, size_t size) +{ + if (!isConnected) { + return 0; + } + + if (data == NULL || size ==0) { + return 0; + } + + size_t tx_len = 0; + required_tx_cnt = 0; + do { + int ans = write(serial_fd, data + tx_len, size-tx_len); + if (ans == -1) { + return tx_len; + } + tx_len += ans; + required_tx_cnt = tx_len; + }while (tx_len(header); + u_int32_t waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(lidar_ans_header) - recvPos; + size_t recvSize; + + int ans = waitForData(remainSize, timeout - waitTime, &recvSize); + if (ans == -2){ + return -2; + }else if (ans == -1){ + return -1; + } + + if(recvSize > remainSize) recvSize = remainSize; + + ans = getData(recvBuffer, recvSize); + if (ans == -1){ + return -2; + } + + for (size_t pos = 0; pos < recvSize; ++pos) { + u_int8_t currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + continue; + } + break; + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE2) { + recvPos = 0; + continue; + } + break; + } + headerBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(lidar_ans_header)) { + return 0; + } + } + } + return -1; +} + +int Flashgo::waitForData(size_t data_count, u_int32_t timeout, size_t * returned_size) +{ + size_t length = 0; + if (returned_size==NULL) { + returned_size=(size_t *)&length; + } + + int max_fd; + fd_set input_set; + struct timeval timeout_val; + + FD_ZERO(&input_set); + FD_SET(serial_fd, &input_set); + max_fd = serial_fd + 1; + + timeout_val.tv_sec = timeout / 1000; + timeout_val.tv_usec = (timeout % 1000) * 1000; + + if ( isConnected ){ + if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { + return -2; + } + if (*returned_size >= data_count){ + return 0; + } + } + + while (isConnected) { + int n = select(max_fd, &input_set, NULL, NULL, &timeout_val); + if (n < 0){ + return -2; + }else if (n == 0) { + return -1; + } else { + assert (FD_ISSET(serial_fd, &input_set)); + if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { + return -2; + } + + if (*returned_size >= data_count){ + return 0; + } else { + int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec; + int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate; + if (remain_timeout > expect_remain_time){ + usleep(expect_remain_time); + } + } + } + } + return -2; +} + +int Flashgo::getHealth(device_health & health, u_int32_t timeout) +{ + int ans; + if (!isConnected) { + return -2; + } + + disableDataGrabbing(); + { + if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_HEALTH)) != 0) { + return ans; + } + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVHEALTH) { + return -3; + } + + if (response_header.size < sizeof(device_health)) { + return -3; + } + + if (waitForData(response_header.size, timeout) != 0) { + return -1; + } + + getData(reinterpret_cast(&health), sizeof(health)); + } + return 0; +} + + +int Flashgo::getDeviceInfo(device_info & info, u_int32_t timeout) +{ + int ans; + if (!isConnected) { + return -2; + } + + disableDataGrabbing(); + { + if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_INFO)) != 0) { + return ans; + } + + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) { + return -3; + } + + if (response_header.size < sizeof(lidar_ans_header)) { + return -3; + } + + if (waitForData(response_header.size, timeout) != 0) { + return -1; + } + getData(reinterpret_cast(&info), sizeof(info)); + } + return 0; +} + +int Flashgo::startScan(bool force, u_int32_t timeout ) +{ + int ans; + if (!isConnected) { + return -2; + } + if (isScanning) { + return 0; + } + + stop(); + { + if ((ans = sendCommand(force?LIDAR_CMD_FORCE_SCAN:LIDAR_CMD_SCAN)) != 0) { + return ans; + } + + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_MEASUREMENT) { + return -3; + } + + if (response_header.size < sizeof(node_info)) { + return -3; + } + + isScanning = true; + ans = this->createThread(); + return ans; + } + return 0; +} + +void * Flashgo::_thread_t(void *args) +{ + Flashgo* pThis = static_cast(args); + pThis->cacheScanData(); + return NULL; +} + +int Flashgo::createThread() +{ + if(pthread_create(&threadId,NULL,_thread_t,(void *)this) != 0) { + return -2; + } + return 0; +} + +int Flashgo::stop() +{ + int ans; + node_info local_buf[128]; + size_t count = 128; + + disableDataGrabbing(); + sendCommand(LIDAR_CMD_FORCE_STOP); + + while(true) { + if ((ans = waitScanData(local_buf, count ,10)) != 0 ) { + if (ans == -1) { + break; + } + }else{ + sendCommand(LIDAR_CMD_FORCE_STOP); + } + } + sendCommand(LIDAR_CMD_STOP); + return 0; +} + +int Flashgo::cacheScanData() +{ + node_info local_buf[128]; + size_t count = 128; + node_info local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + int ans; + memset(local_scan, 0, sizeof(local_scan)); + + waitScanData(local_buf, count); + + while(isScanning) { + if ((ans=waitScanData(local_buf, count)) != 0) { + if (ans != -1) { + isScanning = false; + return -2; + } + } + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT) { + if ((local_scan[0].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + this->lock(); + memcpy(scan_node_buf, local_scan, scan_count*sizeof(node_info)); + scan_node_count = scan_count; + pthread_mutex_unlock(&_lock); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == sizeof(local_scan)) scan_count-=1; + } + } + isScanning = false; + return 0; +} + +#if(ScanDataProtocol == 0) + +int Flashgo::waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout) +{ + if (!isConnected) { + count = 0; + return -2; + } + + size_t recvNodeCount = 0; + u_int32_t startTs = getms(); + u_int32_t waitTime; + int ans; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { + node_info node; + if ((ans = waitNode(&node, timeout - waitTime)) != 0) { + return ans; + } + nodebuffer[recvNodeCount++] = node; + if (recvNodeCount == count) { + return 0; + } + } + count = recvNodeCount; + return -1; +} + +int Flashgo::waitNode(node_info * node, u_int32_t timeout) +{ + int recvPos = 0; + u_int32_t startTs = getms(); + u_int8_t recvBuffer[sizeof(node_info)]; + u_int8_t *nodeBuffer = (u_int8_t*)node; + u_int32_t waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(node_info) - recvPos; + size_t recvSize; + + int ans = waitForData(remainSize, timeout-waitTime, &recvSize); + if (ans == -2) { + return -2; + }else if (ans == -1){ + return -1; + } + + if (recvSize > remainSize) recvSize = remainSize; + + getData(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + u_int8_t currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + { + u_int8_t tmp = (currentByte>>1); + if ( (tmp ^ currentByte) & 0x1 ) { + + } else { + continue; + } + + } + break; + case 1: + { + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + + } else { + recvPos = 0; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(node_info)) { + return 0; + } + } + } + + return -1; +} + +#else + +int Flashgo::waitPackage(node_info * node, u_int32_t timeout) +{ + int recvPos = 0; + int recvPosEnd = 0; + u_int32_t startTs = getms(); + u_int8_t recvBuffer[sizeof(node_package)]; + u_int8_t *nodeBuffer = (u_int8_t*)node; + u_int32_t waitTime; + + static node_package package; + static u_int16_t package_Sample_Index = 0; + static u_int16_t IntervalSampleAngle = 0; + static u_int16_t IntervalSampleAngle_LastPackage = 0; + static u_int16_t FirstSampleAngle = 0; + static u_int16_t LastSampleAngle = 0; + + u_int8_t *packageBuffer = (u_int8_t*)&package.package_Head; + u_int8_t package_Sample_Num = 0; + + int package_recvPos = 0; + + if(package_Sample_Index == 0) { + recvPos = 0; + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = PackagePaidBytes - recvPos; + size_t recvSize; + int ans = waitForData(remainSize, timeout-waitTime, &recvSize); + if (ans == -2){ + return -2; + }else if (ans == -1){ + return -1; + } + + if (recvSize > remainSize) recvSize = remainSize; + + getData(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + u_int8_t currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + { + if ( currentByte == (PH&0xFF) ) { + + } else { + continue; + } + + } + break; + case 1: + { + if ( currentByte == (PH>>8) ) { + + } else { + recvPos = 0; + continue; + } + } + break; + case 2: + { + if ((currentByte == CT_Normal) || (currentByte == CT_RingStart)){ + + } else { + recvPos = 0; + continue; + } + } + break; + case 3: + package_Sample_Num = currentByte; + break; + case 4: + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + FirstSampleAngle = currentByte; + } else { + recvPos = 0; + continue; + } + break; + case 5: + FirstSampleAngle = currentByte*0x100 + FirstSampleAngle; + FirstSampleAngle = FirstSampleAngle>>1; + break; + case 6: + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + LastSampleAngle = currentByte; + } else { + recvPos = 0; + continue; + } + break; + case 7: + LastSampleAngle = currentByte*0x100 + LastSampleAngle; + LastSampleAngle = LastSampleAngle>>1; + if(package_Sample_Num == 1){ + IntervalSampleAngle = 0; + }else{ + if(LastSampleAngle < FirstSampleAngle){ + if((FirstSampleAngle > 270*64) && (LastSampleAngle < 90*64)){ + IntervalSampleAngle = (360*64 + LastSampleAngle - FirstSampleAngle)/(package_Sample_Num-1); + IntervalSampleAngle_LastPackage = IntervalSampleAngle; + } else{ + IntervalSampleAngle = IntervalSampleAngle_LastPackage; + } + } else{ + IntervalSampleAngle = (LastSampleAngle -FirstSampleAngle)/(package_Sample_Num-1); + IntervalSampleAngle_LastPackage = IntervalSampleAngle; + } + } + break; + } + packageBuffer[recvPos++] = currentByte; + } + + if (recvPos == PackagePaidBytes ){ + package_recvPos = recvPos; + break; + } + } + + if(PackagePaidBytes == recvPos){ + startTs = getms(); + recvPos = 0; + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = package_Sample_Num*PackageSampleBytes - recvPos; + size_t recvSize; + int ans =waitForData(remainSize, timeout-waitTime, &recvSize); + if (ans == -2){ + return -2; + }else if (ans == -1){ + return -1; + } + + if (recvSize > remainSize) recvSize = remainSize; + + getData(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + packageBuffer[package_recvPos+recvPos] = recvBuffer[pos]; + recvPos++; + } + if(package_Sample_Num*PackageSampleBytes == recvPos){ + package_recvPos += recvPos; + break; + } + } + if(package_Sample_Num*PackageSampleBytes != recvPos){ + return -1; + } + } else { + return -1; + } + } + + if(package.package_CT == CT_Normal){ + (*node).sync_quality = Node_Default_Quality + Node_NotSync; + } else{ + (*node).sync_quality = Node_Default_Quality + Node_Sync; + } + + (*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT; + (*node).distance_q2 = package.packageSampleDistance[package_Sample_Index]; + + package_Sample_Index++; + if(package_Sample_Index >= package.nowPackageNum){ + package_Sample_Index = 0; + } + return 0; +} + +int Flashgo::waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout) +{ + if (!isConnected) { + count = 0; + return -2; + } + + size_t recvNodeCount = 0; + u_int32_t startTs = getms(); + u_int32_t waitTime; + int ans; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { + node_info node; + if ((ans = this->waitPackage(&node, timeout - waitTime)) != 0) { + return ans; + } + nodebuffer[recvNodeCount++] = node; + + if (recvNodeCount == count) { + return 0; + } + } + count = recvNodeCount; + return -1; +} + +#endif + +int Flashgo::grabScanData(node_info * nodebuffer, size_t & count) +{ + { + if(scan_node_count == 0) { + return -2; + } + size_t size_to_copy = min(count, scan_node_count); + memcpy(nodebuffer, scan_node_buf, size_to_copy*sizeof(node_info)); + count = size_to_copy; + scan_node_count = 0; + } + return 0; +} + +void Flashgo::simpleScanData(std::vector *scan_data , node_info *buffer, size_t count) +{ + scan_data->clear(); + for (int pos = 0; pos < (int)count; ++pos) { + scanDot dot; + if (!buffer[pos].distance_q2) continue; + dot.quality = (buffer[pos].sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + dot.angle = (buffer[pos].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + dot.dist = buffer[pos].distance_q2/4.0f; + scan_data->push_back(dot); + } +} + +int Flashgo::ascendScanData(node_info * nodebuffer, size_t count) +{ + float inc_origin_angle = 360.0/count; + node_info *tmpbuffer = new node_info[count]; + int i = 0; + + for (i = 0; i < (int)count; i++) { + if(nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while(i != 0) { + i--; + float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle; + if (expect_angle < 0.0f) expect_angle = 0.0f; + u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + break; + } + } + + if (i == (int)count) return -3; + + for (i = (int)count - 1; i >= 0; i--) { + if(nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while(i != ((int)count - 1)) { + i++; + float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + break; + } + } + + float frontAngle = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + for (i = 1; i < (int)count; i++) { + if(nodebuffer[i].distance_q2 == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + } + + size_t zero_pos = 0; + float pre_degree = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + + for (i = 1; i < (int)count ; ++i) { + float degree = (nodebuffer[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + if (zero_pos == 0 && (pre_degree - degree > 180)) { + zero_pos = i; + break; + } + pre_degree = degree; + } + + for (i = (int)zero_pos; i < (int)count; i++) { + tmpbuffer[i-zero_pos] = nodebuffer[i]; + } + for (i = 0; i < (int)zero_pos; i++) { + tmpbuffer[i+(int)count-zero_pos] = nodebuffer[i]; + } + + memcpy(nodebuffer, tmpbuffer, count*sizeof(node_info)); + delete tmpbuffer; + + return 0; +} + +int Flashgo::lock(unsigned long timeout) +{ + if (timeout == 0xFFFFFFFF){ + if (pthread_mutex_lock(&_lock) == 0) return 1; + } else if (timeout == 0) { + if (pthread_mutex_trylock(&_lock) == 0) return 1; + } else { + struct timespec wait_time; + timeval now; + gettimeofday(&now,NULL); + + wait_time.tv_sec = timeout/1000 + now.tv_sec; + wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; + + if (wait_time.tv_nsec >= 1000000000) { + ++wait_time.tv_sec; + wait_time.tv_nsec -= 1000000000; + } + switch (pthread_mutex_timedlock(&_lock,&wait_time)) { + case 0: + return 1; + case ETIMEDOUT: + return -1; + } + } + return 0; +} + +void Flashgo::releaseThreadLock() +{ + pthread_mutex_unlock(&_lock); + pthread_mutex_destroy(&_lock); +} diff --git a/src/flashgo_.h b/src/flashgo_.h new file mode 100644 index 0000000..6a4e52d --- /dev/null +++ b/src/flashgo_.h @@ -0,0 +1,164 @@ +#ifndef FLASHGO_H +#define FLASHGO_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#define LIDAR_CMD_STOP 0x25 +#define LIDAR_CMD_SCAN 0x20 +#define LIDAR_CMD_FORCE_SCAN 0x21 +#define LIDAR_CMD_FORCE_STOP 0x00 +#define LIDAR_CMD_GET_DEVICE_INFO 0x50 +#define LIDAR_CMD_GET_DEVICE_HEALTH 0x52 +#define LIDAR_ANS_TYPE_DEVINFO 0x4 +#define LIDAR_ANS_TYPE_DEVHEALTH 0x6 +#define LIDAR_CMD_SYNC_BYTE 0xA5 +#define LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 +#define LIDAR_ANS_SYNC_BYTE1 0xA5 +#define LIDAR_ANS_SYNC_BYTE2 0x5A +#define LIDAR_ANS_TYPE_MEASUREMENT 0x81 +#define LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) +#define LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 +#define LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) +#define LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 + +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +#define ScanDataProtocol 0 +#define PrintfEnable 0 +#define PackageSampleMaxLngth 0x100 +typedef enum +{ + CT_Normal = 0, + CT_RingStart = 1, + CT_Tail, +}CT; +#define Node_Default_Quality (10<<2) +#define Node_Sync 1 +#define Node_NotSync 2 +#define PackagePaidBytes 8 +#define PackageSampleBytes 2 +#define PH 0x55AA + +struct node_info{ + u_int8_t sync_quality; + u_int16_t angle_q6_checkbit; + u_int16_t distance_q2; +} __attribute__((packed)) ; + +struct node_package { + u_int16_t package_Head; + u_int8_t package_CT; + u_int8_t nowPackageNum; + u_int16_t packageFirstSampleAngle; + u_int16_t packageLastSampleAngle; + u_int16_t packageSampleDistance[PackageSampleMaxLngth]; +} __attribute__((packed)) ; + +struct device_info{ + u_int8_t model; + u_int16_t firmware_version; + u_int8_t hardware_version; + u_int8_t serialnum[16]; +} __attribute__((packed)) ; + + struct device_health { + u_int8_t status; + u_int16_t error_code; +} __attribute__((packed)) ; + + struct cmd_packet { + u_int8_t syncByte; + u_int8_t cmd_flag; + u_int8_t size; + u_int8_t data[0]; +} __attribute__((packed)) ; + + struct lidar_ans_header { + u_int8_t syncByte1; + u_int8_t syncByte2; + u_int32_t size:30; + u_int32_t subType:2; + u_int8_t type; +} __attribute__((packed)); + +struct scanDot { + u_int8_t quality; + float angle; + float dist; + }; + +class Flashgo +{ +public: + Flashgo(); + int connect(const char * port_path, u_int32_t baudrate); + void disconnect(); + int getHealth(device_health & health, u_int32_t timeout = DEFAULT_TIMEOUT); + int getDeviceInfo(device_info & info, u_int32_t timeout = DEFAULT_TIMEOUT); + int startScan(bool force = false, u_int32_t timeout = DEFAULT_TIMEOUT) ; + int stop(); + int grabScanData(node_info * nodebuffer, size_t & count) ; + int ascendScanData(node_info * nodebuffer, size_t count); + int createThread(); + u_int32_t getms(); + void simpleScanData(std::vector * scan_data , node_info *buffer, size_t count); + int lock(unsigned long timeout = 0xFFFFFFFF); + void releaseThreadLock(); + +protected: +#if(ScanDataProtocol == 0) + int waitNode(node_info * node, u_int32_t timeout = DEFAULT_TIMEOUT); +#else + int waitPackage(node_info * node, u_int32_t timeout = DEFAULT_TIMEOUT); +#endif + int waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout = DEFAULT_TIMEOUT); + int cacheScanData(); + int sendCommand(u_int8_t cmd, const void * payload = NULL, size_t payloadsize = 0); + int waitResponseHeader(lidar_ans_header * header, u_int32_t timeout = DEFAULT_TIMEOUT); + u_int32_t getTermBaudBitmap(u_int32_t baud); + int waitForData(size_t data_count,u_int32_t timeout = -1, size_t * returned_size = NULL); + int getData(unsigned char * data, size_t size); + int sendData(const unsigned char * data, size_t size); + void disableDataGrabbing(); + static void* _thread_t(void* args); + +public: + bool isConnected; + bool isScanning; + enum { + DEFAULT_TIMEOUT = 2000, //2000 ms + MAX_SCAN_NODES = 2048, + }; + node_info scan_node_buf[2048]; + size_t scan_node_count; + pthread_mutex_t _lock; +}; + +#endif // FLASHGO_H diff --git a/src/flashgo_client.cpp b/src/flashgo_client.cpp new file mode 100644 index 0000000..b4ee39f --- /dev/null +++ b/src/flashgo_client.cpp @@ -0,0 +1,39 @@ + +/* + * FLASH LIDAR System + * Flash Lidar ROS Node client + * + * Copyright 2015 - 2016 EAI Team + * http://www.eaibot.com + * + */ + + +#include "ros/ros.h" +#include "sensor_msgs/LaserScan.h" + +#define RAD2DEG(x) ((x)*180./M_PI) + +void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) +{ + int count = scan->scan_time / scan->time_increment; + ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count); + ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max)); + + for(int i = 0; i < count; i++) { + float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); + ROS_INFO(": [%f, %f]", degree, scan->ranges[i]); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "flash_lidar_node_client"); + ros::NodeHandle n; + + ros::Subscriber sub = n.subscribe("/scan", 1000, scanCallback); + + ros::spin(); + + return 0; +} diff --git a/src/flashgo_node.cpp b/src/flashgo_node.cpp new file mode 100644 index 0000000..dd9b545 --- /dev/null +++ b/src/flashgo_node.cpp @@ -0,0 +1,256 @@ + +/* + * FLASH LIDAR System + * Flash Lidar ROS Node client + * + * Copyright 2015 - 2016 EAI Team + * http://www.eaibot.com + * + */ + + + +#include "ros/ros.h" +#include "sensor_msgs/LaserScan.h" + +#include "flashgo.h" + +#ifndef _countof +#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) +#endif + +#define DEG2RAD(x) ((x)*M_PI/180.) + +void publish_scan(ros::Publisher *pub, + node_info *nodes, + size_t node_count, ros::Time start, + double scan_time, bool inverted, + float angle_min, float angle_max, + std::string frame_id) +{ + static int scan_count = 0; + sensor_msgs::LaserScan scan_msg; + + scan_msg.header.stamp = start; + scan_msg.header.frame_id = frame_id; + scan_count++; + + scan_msg.angle_min = angle_min; + scan_msg.angle_max = angle_max; + scan_msg.angle_increment = (scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1); + + scan_msg.scan_time = scan_time; + scan_msg.time_increment = scan_time / (double)(node_count-1); + + scan_msg.range_min = 0.15; + scan_msg.range_max = 6.; + + scan_msg.ranges.resize(node_count); + if (!inverted) { // assumes scan window at the top + for (size_t i = 0; i < node_count; i++) { + scan_msg.ranges[i] = (float)nodes[i].distance_q2/4.0f/1000; + } + } else { + for (size_t i = 0; i < node_count; i++) { + scan_msg.ranges[node_count-1-i] = (float)nodes[i].distance_q2/4.0f/1000; + } + } + + scan_msg.intensities.resize(node_count); + for (size_t i = 0; i < node_count; i++) { + scan_msg.intensities[i] = (float)0; + } + + for(size_t i=0; i < scan_msg.ranges.size(); i++){ + if(scan_msg.ranges[i] < 0.3){ + scan_msg.ranges[i] = 0; + } + } + + pub->publish(scan_msg); +} + +bool checkFlashLidarHealth(Flashgo * drv) +{ + int op_result; + device_health healthinfo; + + op_result = drv->getHealth(healthinfo); + if (op_result == 0) { + printf("Flash Lidar health status : %d\n", healthinfo.status); + + if (healthinfo.status == 2) { + fprintf(stderr, "Error, Flash Lidar internal error detected." + "Please reboot the device to retry.\n"); + return false; + } else { + return true; + } + + } else { + fprintf(stderr, "Error, cannot retrieve Flash Lidar health code: %x\n", op_result); + return false; + } +} + +int main(int argc, char * argv[]) { + ros::init(argc, argv, "flash_lidar_node"); + + std::string serial_port; + int serial_baudrate; + std::string frame_id; + bool inverted; + bool angle_compensate; + + ros::NodeHandle nh; + ros::Publisher scan_pub = nh.advertise("scan", 1000); + ros::NodeHandle nh_private("~"); + nh_private.param("serial_port", serial_port, "/dev/ttyUSB0"); + nh_private.param("serial_baudrate", serial_baudrate, 230400); + nh_private.param("frame_id", frame_id, "laser_frame"); + nh_private.param("inverted", inverted, "false"); + nh_private.param("angle_compensate", angle_compensate, "true"); + + u_int32_t op_result; + + // create the driver instance + Flashgo * drv = new Flashgo(); + + if (!drv) { + fprintf(stderr, "Create Driver fail, exit\n"); + return -2; + } + if (serial_baudrate == 115200 ) { + drv->setScanDataProtocol(1); + }else{ + drv->setScanDataProtocol(0); + } + // make connection... + op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); + if (op_result == -1) { + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + //flashgo::~flashgo(); + return -1; + }else{ + int isEAI = drv->getEAI(); + if(isEAI != 0){ + drv->disconnect(); + if(115200 == (u_int32_t)serial_baudrate){ + drv->setScanDataProtocol(0); + serial_baudrate=230400; + op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); + if (op_result == -1) { + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + }else{ + isEAI = drv->getEAI(); + if(isEAI != 0){ + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + } + } + }else{ + drv->setScanDataProtocol(1); + serial_baudrate=115200; + op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); + if (op_result == -1) { + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + }else{ + isEAI = drv->getEAI(); + if(isEAI != 0){ + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + } + } + } + } + } + + // check health... + if (!checkFlashLidarHealth(drv)) { + //flashgo::~flashgo(); + return -1; + } + + // start scan... + drv->startScan(); + + ros::Time start_scan_time; + ros::Time end_scan_time; + double scan_duration; + while (ros::ok()) { + + node_info nodes[360*2]; + size_t count = _countof(nodes); + + start_scan_time = ros::Time::now(); + op_result = drv->grabScanData(nodes, count); + end_scan_time = ros::Time::now(); + scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3; + + if (op_result == 0) { + op_result = drv->ascendScanData(nodes, count); + + float angle_min = DEG2RAD(0.0f); + float angle_max = DEG2RAD(359.0f); + if (op_result == 0) { + if (angle_compensate) { + const int angle_compensate_nodes_count = 360; + const int angle_compensate_multiple = 1; + int angle_compensate_offset = 0; + node_info angle_compensate_nodes[angle_compensate_nodes_count]; + memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(node_info)); + int i = 0, j = 0; + for( ; i < count; i++ ) { + if (nodes[i].distance_q2 != 0) { + float angle = (float)((nodes[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); + int angle_value = (int)(angle * angle_compensate_multiple); + if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value; + for (j = 0; j < angle_compensate_multiple; j++) { + angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i]; + } + } + } + + publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, + frame_id); + } else { + int start_node = 0, end_node = 0; + int i = 0; + // find the first valid node and last valid node + while (nodes[i++].distance_q2 == 0); + start_node = i-1; + i = count -1; + while (nodes[i--].distance_q2 == 0); + end_node = i+1; + + angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); + angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); + + publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, + frame_id); + } + } else if (op_result == -2) { + // All the data is invalid, just publish them + float angle_min = DEG2RAD(0.0f); + float angle_max = DEG2RAD(359.0f); + + publish_scan(&scan_pub, nodes, count, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, + frame_id); + } + } + + ros::spinOnce(); + } + + // done! + drv->disconnect(); + return 0; +} From 5d307321dddc7a70f7ce29b40bb138a68def5a74 Mon Sep 17 00:00:00 2001 From: EAIBOT Date: Fri, 4 Nov 2016 16:36:27 +0800 Subject: [PATCH 2/4] Delete flashgo_.cpp --- src/flashgo_.cpp | 917 ----------------------------------------------- 1 file changed, 917 deletions(-) delete mode 100644 src/flashgo_.cpp diff --git a/src/flashgo_.cpp b/src/flashgo_.cpp deleted file mode 100644 index a1285b7..0000000 --- a/src/flashgo_.cpp +++ /dev/null @@ -1,917 +0,0 @@ -#include "flashgo.h" - -static int serial_fd; -static pthread_t threadId; -size_t required_tx_cnt; -size_t required_rx_cnt; -u_int32_t _baudrate; - -Flashgo::Flashgo() -{ - isConnected = false; - isScanning = false; - pthread_mutex_init(&_lock, NULL); -} - -int Flashgo::connect(const char * port_path, u_int32_t baudrate) -{ - _baudrate = baudrate; - if (isConnected){ - return 1; - } - - { - if (serial_fd != -1) { - close(serial_fd); - } - - serial_fd = -1; - serial_fd = open(port_path, O_RDWR | O_NOCTTY | O_NDELAY); - if (serial_fd == -1) { - return -1; - } - - struct termios options, oldopt; - tcgetattr(serial_fd, &oldopt); - bzero(&options,sizeof(struct termios)); - - u_int32_t termbaud = getTermBaudBitmap(baudrate); - if (termbaud == (u_int32_t)-1) { - if (serial_fd != -1) { - close(serial_fd); - } - serial_fd = -1; - return -1; - } - cfsetispeed(&options, termbaud); - cfsetospeed(&options, termbaud); - - options.c_cflag |= (CLOCAL | CREAD); - options.c_cflag &= ~PARENB; - options.c_cflag &= ~CSTOPB; - options.c_cflag &= ~CSIZE; - options.c_cflag |= CS8; - options.c_iflag &= ~(IXON | IXOFF | IXANY); - options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); - options.c_oflag &= ~OPOST; - - tcflush(serial_fd,TCIFLUSH); - if (fcntl(serial_fd, F_SETFL, FNDELAY)) { - if (serial_fd != -1) { - close(serial_fd); - } - serial_fd = -1; - return -1; - } - if (tcsetattr(serial_fd, TCSANOW, &options)) { - if (serial_fd != -1) { - close(serial_fd); - } - serial_fd = -1; - return -1; - } - - u_int32_t dtr_bit = TIOCM_DTR; - ioctl(serial_fd, TIOCMBIC, &dtr_bit); - } - - isConnected = true; - return serial_fd; -} - - -void Flashgo::disconnect() -{ - if (!isConnected){ - return ; - } - stop(); - if (serial_fd != -1) { - close(serial_fd); - } - isConnected = false; - serial_fd = -1; -} - -u_int32_t Flashgo::getTermBaudBitmap(u_int32_t baud) -{ - #define BAUD_CONV( _baud_) case _baud_: return B##_baud_ - switch (baud) { - BAUD_CONV(1200); - BAUD_CONV(1800); - BAUD_CONV(2400); - BAUD_CONV(4800); - BAUD_CONV(9600); - BAUD_CONV(19200); - BAUD_CONV(38400); - BAUD_CONV(57600); - BAUD_CONV(115200); - BAUD_CONV(230400); - BAUD_CONV(460800); - BAUD_CONV(500000); - BAUD_CONV(576000); - BAUD_CONV(921600); - BAUD_CONV(1000000); - BAUD_CONV(1152000); - BAUD_CONV(1500000); - BAUD_CONV(2000000); - BAUD_CONV(2500000); - BAUD_CONV(3000000); - BAUD_CONV(3500000); - BAUD_CONV(4000000); - } - return -1; -} - -void Flashgo::disableDataGrabbing() -{ - isScanning = false; - pthread_join(threadId , NULL); -} - -int Flashgo::sendCommand(u_int8_t cmd, const void * payload, size_t payloadsize) -{ - u_int8_t pkt_header[10]; - cmd_packet * header = reinterpret_cast(pkt_header); - u_int8_t checksum = 0; - - if (!isConnected) { - return -2; - } - if (payloadsize && payload) { - cmd |= LIDAR_CMDFLAG_HAS_PAYLOAD; - } - - header->syncByte = LIDAR_CMD_SYNC_BYTE; - header->cmd_flag = cmd; - sendData(pkt_header, 2) ; - - if (cmd & LIDAR_CMDFLAG_HAS_PAYLOAD) { - checksum ^= LIDAR_CMD_SYNC_BYTE; - checksum ^= cmd; - checksum ^= (payloadsize & 0xFF); - - for (size_t pos = 0; pos < payloadsize; ++pos) { - checksum ^= ((u_int8_t *)payload)[pos]; - } - - u_int8_t sizebyte = payloadsize; - sendData(&sizebyte, 1); - - sendData((const u_int8_t *)payload, sizebyte); - - sendData(&checksum, 1); - } - return 0; -} - -int Flashgo::sendData(const unsigned char * data, size_t size) -{ - if (!isConnected) { - return 0; - } - - if (data == NULL || size ==0) { - return 0; - } - - size_t tx_len = 0; - required_tx_cnt = 0; - do { - int ans = write(serial_fd, data + tx_len, size-tx_len); - if (ans == -1) { - return tx_len; - } - tx_len += ans; - required_tx_cnt = tx_len; - }while (tx_len(header); - u_int32_t waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(lidar_ans_header) - recvPos; - size_t recvSize; - - int ans = waitForData(remainSize, timeout - waitTime, &recvSize); - if (ans == -2){ - return -2; - }else if (ans == -1){ - return -1; - } - - if(recvSize > remainSize) recvSize = remainSize; - - ans = getData(recvBuffer, recvSize); - if (ans == -1){ - return -2; - } - - for (size_t pos = 0; pos < recvSize; ++pos) { - u_int8_t currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: - if (currentByte != LIDAR_ANS_SYNC_BYTE1) { - continue; - } - break; - case 1: - if (currentByte != LIDAR_ANS_SYNC_BYTE2) { - recvPos = 0; - continue; - } - break; - } - headerBuffer[recvPos++] = currentByte; - - if (recvPos == sizeof(lidar_ans_header)) { - return 0; - } - } - } - return -1; -} - -int Flashgo::waitForData(size_t data_count, u_int32_t timeout, size_t * returned_size) -{ - size_t length = 0; - if (returned_size==NULL) { - returned_size=(size_t *)&length; - } - - int max_fd; - fd_set input_set; - struct timeval timeout_val; - - FD_ZERO(&input_set); - FD_SET(serial_fd, &input_set); - max_fd = serial_fd + 1; - - timeout_val.tv_sec = timeout / 1000; - timeout_val.tv_usec = (timeout % 1000) * 1000; - - if ( isConnected ){ - if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { - return -2; - } - if (*returned_size >= data_count){ - return 0; - } - } - - while (isConnected) { - int n = select(max_fd, &input_set, NULL, NULL, &timeout_val); - if (n < 0){ - return -2; - }else if (n == 0) { - return -1; - } else { - assert (FD_ISSET(serial_fd, &input_set)); - if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) { - return -2; - } - - if (*returned_size >= data_count){ - return 0; - } else { - int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec; - int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate; - if (remain_timeout > expect_remain_time){ - usleep(expect_remain_time); - } - } - } - } - return -2; -} - -int Flashgo::getHealth(device_health & health, u_int32_t timeout) -{ - int ans; - if (!isConnected) { - return -2; - } - - disableDataGrabbing(); - { - if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_HEALTH)) != 0) { - return ans; - } - lidar_ans_header response_header; - if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { - return ans; - } - - if (response_header.type != LIDAR_ANS_TYPE_DEVHEALTH) { - return -3; - } - - if (response_header.size < sizeof(device_health)) { - return -3; - } - - if (waitForData(response_header.size, timeout) != 0) { - return -1; - } - - getData(reinterpret_cast(&health), sizeof(health)); - } - return 0; -} - - -int Flashgo::getDeviceInfo(device_info & info, u_int32_t timeout) -{ - int ans; - if (!isConnected) { - return -2; - } - - disableDataGrabbing(); - { - if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_INFO)) != 0) { - return ans; - } - - lidar_ans_header response_header; - if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { - return ans; - } - - if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) { - return -3; - } - - if (response_header.size < sizeof(lidar_ans_header)) { - return -3; - } - - if (waitForData(response_header.size, timeout) != 0) { - return -1; - } - getData(reinterpret_cast(&info), sizeof(info)); - } - return 0; -} - -int Flashgo::startScan(bool force, u_int32_t timeout ) -{ - int ans; - if (!isConnected) { - return -2; - } - if (isScanning) { - return 0; - } - - stop(); - { - if ((ans = sendCommand(force?LIDAR_CMD_FORCE_SCAN:LIDAR_CMD_SCAN)) != 0) { - return ans; - } - - lidar_ans_header response_header; - if ((ans = waitResponseHeader(&response_header, timeout)) != 0) { - return ans; - } - - if (response_header.type != LIDAR_ANS_TYPE_MEASUREMENT) { - return -3; - } - - if (response_header.size < sizeof(node_info)) { - return -3; - } - - isScanning = true; - ans = this->createThread(); - return ans; - } - return 0; -} - -void * Flashgo::_thread_t(void *args) -{ - Flashgo* pThis = static_cast(args); - pThis->cacheScanData(); - return NULL; -} - -int Flashgo::createThread() -{ - if(pthread_create(&threadId,NULL,_thread_t,(void *)this) != 0) { - return -2; - } - return 0; -} - -int Flashgo::stop() -{ - int ans; - node_info local_buf[128]; - size_t count = 128; - - disableDataGrabbing(); - sendCommand(LIDAR_CMD_FORCE_STOP); - - while(true) { - if ((ans = waitScanData(local_buf, count ,10)) != 0 ) { - if (ans == -1) { - break; - } - }else{ - sendCommand(LIDAR_CMD_FORCE_STOP); - } - } - sendCommand(LIDAR_CMD_STOP); - return 0; -} - -int Flashgo::cacheScanData() -{ - node_info local_buf[128]; - size_t count = 128; - node_info local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - int ans; - memset(local_scan, 0, sizeof(local_scan)); - - waitScanData(local_buf, count); - - while(isScanning) { - if ((ans=waitScanData(local_buf, count)) != 0) { - if (ans != -1) { - isScanning = false; - return -2; - } - } - - for (size_t pos = 0; pos < count; ++pos) { - if (local_buf[pos].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT) { - if ((local_scan[0].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT)) { - this->lock(); - memcpy(scan_node_buf, local_scan, scan_count*sizeof(node_info)); - scan_node_count = scan_count; - pthread_mutex_unlock(&_lock); - } - scan_count = 0; - } - local_scan[scan_count++] = local_buf[pos]; - if (scan_count == sizeof(local_scan)) scan_count-=1; - } - } - isScanning = false; - return 0; -} - -#if(ScanDataProtocol == 0) - -int Flashgo::waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout) -{ - if (!isConnected) { - count = 0; - return -2; - } - - size_t recvNodeCount = 0; - u_int32_t startTs = getms(); - u_int32_t waitTime; - int ans; - - while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { - node_info node; - if ((ans = waitNode(&node, timeout - waitTime)) != 0) { - return ans; - } - nodebuffer[recvNodeCount++] = node; - if (recvNodeCount == count) { - return 0; - } - } - count = recvNodeCount; - return -1; -} - -int Flashgo::waitNode(node_info * node, u_int32_t timeout) -{ - int recvPos = 0; - u_int32_t startTs = getms(); - u_int8_t recvBuffer[sizeof(node_info)]; - u_int8_t *nodeBuffer = (u_int8_t*)node; - u_int32_t waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(node_info) - recvPos; - size_t recvSize; - - int ans = waitForData(remainSize, timeout-waitTime, &recvSize); - if (ans == -2) { - return -2; - }else if (ans == -1){ - return -1; - } - - if (recvSize > remainSize) recvSize = remainSize; - - getData(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - u_int8_t currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: - { - u_int8_t tmp = (currentByte>>1); - if ( (tmp ^ currentByte) & 0x1 ) { - - } else { - continue; - } - - } - break; - case 1: - { - if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { - - } else { - recvPos = 0; - continue; - } - } - break; - } - nodeBuffer[recvPos++] = currentByte; - - if (recvPos == sizeof(node_info)) { - return 0; - } - } - } - - return -1; -} - -#else - -int Flashgo::waitPackage(node_info * node, u_int32_t timeout) -{ - int recvPos = 0; - int recvPosEnd = 0; - u_int32_t startTs = getms(); - u_int8_t recvBuffer[sizeof(node_package)]; - u_int8_t *nodeBuffer = (u_int8_t*)node; - u_int32_t waitTime; - - static node_package package; - static u_int16_t package_Sample_Index = 0; - static u_int16_t IntervalSampleAngle = 0; - static u_int16_t IntervalSampleAngle_LastPackage = 0; - static u_int16_t FirstSampleAngle = 0; - static u_int16_t LastSampleAngle = 0; - - u_int8_t *packageBuffer = (u_int8_t*)&package.package_Head; - u_int8_t package_Sample_Num = 0; - - int package_recvPos = 0; - - if(package_Sample_Index == 0) { - recvPos = 0; - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = PackagePaidBytes - recvPos; - size_t recvSize; - int ans = waitForData(remainSize, timeout-waitTime, &recvSize); - if (ans == -2){ - return -2; - }else if (ans == -1){ - return -1; - } - - if (recvSize > remainSize) recvSize = remainSize; - - getData(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - u_int8_t currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: - { - if ( currentByte == (PH&0xFF) ) { - - } else { - continue; - } - - } - break; - case 1: - { - if ( currentByte == (PH>>8) ) { - - } else { - recvPos = 0; - continue; - } - } - break; - case 2: - { - if ((currentByte == CT_Normal) || (currentByte == CT_RingStart)){ - - } else { - recvPos = 0; - continue; - } - } - break; - case 3: - package_Sample_Num = currentByte; - break; - case 4: - if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { - FirstSampleAngle = currentByte; - } else { - recvPos = 0; - continue; - } - break; - case 5: - FirstSampleAngle = currentByte*0x100 + FirstSampleAngle; - FirstSampleAngle = FirstSampleAngle>>1; - break; - case 6: - if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { - LastSampleAngle = currentByte; - } else { - recvPos = 0; - continue; - } - break; - case 7: - LastSampleAngle = currentByte*0x100 + LastSampleAngle; - LastSampleAngle = LastSampleAngle>>1; - if(package_Sample_Num == 1){ - IntervalSampleAngle = 0; - }else{ - if(LastSampleAngle < FirstSampleAngle){ - if((FirstSampleAngle > 270*64) && (LastSampleAngle < 90*64)){ - IntervalSampleAngle = (360*64 + LastSampleAngle - FirstSampleAngle)/(package_Sample_Num-1); - IntervalSampleAngle_LastPackage = IntervalSampleAngle; - } else{ - IntervalSampleAngle = IntervalSampleAngle_LastPackage; - } - } else{ - IntervalSampleAngle = (LastSampleAngle -FirstSampleAngle)/(package_Sample_Num-1); - IntervalSampleAngle_LastPackage = IntervalSampleAngle; - } - } - break; - } - packageBuffer[recvPos++] = currentByte; - } - - if (recvPos == PackagePaidBytes ){ - package_recvPos = recvPos; - break; - } - } - - if(PackagePaidBytes == recvPos){ - startTs = getms(); - recvPos = 0; - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = package_Sample_Num*PackageSampleBytes - recvPos; - size_t recvSize; - int ans =waitForData(remainSize, timeout-waitTime, &recvSize); - if (ans == -2){ - return -2; - }else if (ans == -1){ - return -1; - } - - if (recvSize > remainSize) recvSize = remainSize; - - getData(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - packageBuffer[package_recvPos+recvPos] = recvBuffer[pos]; - recvPos++; - } - if(package_Sample_Num*PackageSampleBytes == recvPos){ - package_recvPos += recvPos; - break; - } - } - if(package_Sample_Num*PackageSampleBytes != recvPos){ - return -1; - } - } else { - return -1; - } - } - - if(package.package_CT == CT_Normal){ - (*node).sync_quality = Node_Default_Quality + Node_NotSync; - } else{ - (*node).sync_quality = Node_Default_Quality + Node_Sync; - } - - (*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT; - (*node).distance_q2 = package.packageSampleDistance[package_Sample_Index]; - - package_Sample_Index++; - if(package_Sample_Index >= package.nowPackageNum){ - package_Sample_Index = 0; - } - return 0; -} - -int Flashgo::waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout) -{ - if (!isConnected) { - count = 0; - return -2; - } - - size_t recvNodeCount = 0; - u_int32_t startTs = getms(); - u_int32_t waitTime; - int ans; - - while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { - node_info node; - if ((ans = this->waitPackage(&node, timeout - waitTime)) != 0) { - return ans; - } - nodebuffer[recvNodeCount++] = node; - - if (recvNodeCount == count) { - return 0; - } - } - count = recvNodeCount; - return -1; -} - -#endif - -int Flashgo::grabScanData(node_info * nodebuffer, size_t & count) -{ - { - if(scan_node_count == 0) { - return -2; - } - size_t size_to_copy = min(count, scan_node_count); - memcpy(nodebuffer, scan_node_buf, size_to_copy*sizeof(node_info)); - count = size_to_copy; - scan_node_count = 0; - } - return 0; -} - -void Flashgo::simpleScanData(std::vector *scan_data , node_info *buffer, size_t count) -{ - scan_data->clear(); - for (int pos = 0; pos < (int)count; ++pos) { - scanDot dot; - if (!buffer[pos].distance_q2) continue; - dot.quality = (buffer[pos].sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); - dot.angle = (buffer[pos].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - dot.dist = buffer[pos].distance_q2/4.0f; - scan_data->push_back(dot); - } -} - -int Flashgo::ascendScanData(node_info * nodebuffer, size_t count) -{ - float inc_origin_angle = 360.0/count; - node_info *tmpbuffer = new node_info[count]; - int i = 0; - - for (i = 0; i < (int)count; i++) { - if(nodebuffer[i].distance_q2 == 0) { - continue; - } else { - while(i != 0) { - i--; - float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle; - if (expect_angle < 0.0f) expect_angle = 0.0f; - u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; - nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; - } - break; - } - } - - if (i == (int)count) return -3; - - for (i = (int)count - 1; i >= 0; i--) { - if(nodebuffer[i].distance_q2 == 0) { - continue; - } else { - while(i != ((int)count - 1)) { - i++; - float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; - nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; - } - break; - } - } - - float frontAngle = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - for (i = 1; i < (int)count; i++) { - if(nodebuffer[i].distance_q2 == 0) { - float expect_angle = frontAngle + i * inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - u_int16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT; - nodebuffer[i].angle_q6_checkbit = (((u_int16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; - } - } - - size_t zero_pos = 0; - float pre_degree = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - - for (i = 1; i < (int)count ; ++i) { - float degree = (nodebuffer[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - if (zero_pos == 0 && (pre_degree - degree > 180)) { - zero_pos = i; - break; - } - pre_degree = degree; - } - - for (i = (int)zero_pos; i < (int)count; i++) { - tmpbuffer[i-zero_pos] = nodebuffer[i]; - } - for (i = 0; i < (int)zero_pos; i++) { - tmpbuffer[i+(int)count-zero_pos] = nodebuffer[i]; - } - - memcpy(nodebuffer, tmpbuffer, count*sizeof(node_info)); - delete tmpbuffer; - - return 0; -} - -int Flashgo::lock(unsigned long timeout) -{ - if (timeout == 0xFFFFFFFF){ - if (pthread_mutex_lock(&_lock) == 0) return 1; - } else if (timeout == 0) { - if (pthread_mutex_trylock(&_lock) == 0) return 1; - } else { - struct timespec wait_time; - timeval now; - gettimeofday(&now,NULL); - - wait_time.tv_sec = timeout/1000 + now.tv_sec; - wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; - - if (wait_time.tv_nsec >= 1000000000) { - ++wait_time.tv_sec; - wait_time.tv_nsec -= 1000000000; - } - switch (pthread_mutex_timedlock(&_lock,&wait_time)) { - case 0: - return 1; - case ETIMEDOUT: - return -1; - } - } - return 0; -} - -void Flashgo::releaseThreadLock() -{ - pthread_mutex_unlock(&_lock); - pthread_mutex_destroy(&_lock); -} From 5452d8539d00984f235a7326373aa18741e21bb8 Mon Sep 17 00:00:00 2001 From: EAIBOT Date: Fri, 4 Nov 2016 16:36:36 +0800 Subject: [PATCH 3/4] Delete flashgo_.h --- src/flashgo_.h | 164 ------------------------------------------------- 1 file changed, 164 deletions(-) delete mode 100644 src/flashgo_.h diff --git a/src/flashgo_.h b/src/flashgo_.h deleted file mode 100644 index 6a4e52d..0000000 --- a/src/flashgo_.h +++ /dev/null @@ -1,164 +0,0 @@ -#ifndef FLASHGO_H -#define FLASHGO_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#define LIDAR_CMD_STOP 0x25 -#define LIDAR_CMD_SCAN 0x20 -#define LIDAR_CMD_FORCE_SCAN 0x21 -#define LIDAR_CMD_FORCE_STOP 0x00 -#define LIDAR_CMD_GET_DEVICE_INFO 0x50 -#define LIDAR_CMD_GET_DEVICE_HEALTH 0x52 -#define LIDAR_ANS_TYPE_DEVINFO 0x4 -#define LIDAR_ANS_TYPE_DEVHEALTH 0x6 -#define LIDAR_CMD_SYNC_BYTE 0xA5 -#define LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 -#define LIDAR_ANS_SYNC_BYTE1 0xA5 -#define LIDAR_ANS_SYNC_BYTE2 0x5A -#define LIDAR_ANS_TYPE_MEASUREMENT 0x81 -#define LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) -#define LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 -#define LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) -#define LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 - -#ifndef min -#define min(a,b) (((a) < (b)) ? (a) : (b)) -#endif - -#define ScanDataProtocol 0 -#define PrintfEnable 0 -#define PackageSampleMaxLngth 0x100 -typedef enum -{ - CT_Normal = 0, - CT_RingStart = 1, - CT_Tail, -}CT; -#define Node_Default_Quality (10<<2) -#define Node_Sync 1 -#define Node_NotSync 2 -#define PackagePaidBytes 8 -#define PackageSampleBytes 2 -#define PH 0x55AA - -struct node_info{ - u_int8_t sync_quality; - u_int16_t angle_q6_checkbit; - u_int16_t distance_q2; -} __attribute__((packed)) ; - -struct node_package { - u_int16_t package_Head; - u_int8_t package_CT; - u_int8_t nowPackageNum; - u_int16_t packageFirstSampleAngle; - u_int16_t packageLastSampleAngle; - u_int16_t packageSampleDistance[PackageSampleMaxLngth]; -} __attribute__((packed)) ; - -struct device_info{ - u_int8_t model; - u_int16_t firmware_version; - u_int8_t hardware_version; - u_int8_t serialnum[16]; -} __attribute__((packed)) ; - - struct device_health { - u_int8_t status; - u_int16_t error_code; -} __attribute__((packed)) ; - - struct cmd_packet { - u_int8_t syncByte; - u_int8_t cmd_flag; - u_int8_t size; - u_int8_t data[0]; -} __attribute__((packed)) ; - - struct lidar_ans_header { - u_int8_t syncByte1; - u_int8_t syncByte2; - u_int32_t size:30; - u_int32_t subType:2; - u_int8_t type; -} __attribute__((packed)); - -struct scanDot { - u_int8_t quality; - float angle; - float dist; - }; - -class Flashgo -{ -public: - Flashgo(); - int connect(const char * port_path, u_int32_t baudrate); - void disconnect(); - int getHealth(device_health & health, u_int32_t timeout = DEFAULT_TIMEOUT); - int getDeviceInfo(device_info & info, u_int32_t timeout = DEFAULT_TIMEOUT); - int startScan(bool force = false, u_int32_t timeout = DEFAULT_TIMEOUT) ; - int stop(); - int grabScanData(node_info * nodebuffer, size_t & count) ; - int ascendScanData(node_info * nodebuffer, size_t count); - int createThread(); - u_int32_t getms(); - void simpleScanData(std::vector * scan_data , node_info *buffer, size_t count); - int lock(unsigned long timeout = 0xFFFFFFFF); - void releaseThreadLock(); - -protected: -#if(ScanDataProtocol == 0) - int waitNode(node_info * node, u_int32_t timeout = DEFAULT_TIMEOUT); -#else - int waitPackage(node_info * node, u_int32_t timeout = DEFAULT_TIMEOUT); -#endif - int waitScanData(node_info * nodebuffer, size_t & count, u_int32_t timeout = DEFAULT_TIMEOUT); - int cacheScanData(); - int sendCommand(u_int8_t cmd, const void * payload = NULL, size_t payloadsize = 0); - int waitResponseHeader(lidar_ans_header * header, u_int32_t timeout = DEFAULT_TIMEOUT); - u_int32_t getTermBaudBitmap(u_int32_t baud); - int waitForData(size_t data_count,u_int32_t timeout = -1, size_t * returned_size = NULL); - int getData(unsigned char * data, size_t size); - int sendData(const unsigned char * data, size_t size); - void disableDataGrabbing(); - static void* _thread_t(void* args); - -public: - bool isConnected; - bool isScanning; - enum { - DEFAULT_TIMEOUT = 2000, //2000 ms - MAX_SCAN_NODES = 2048, - }; - node_info scan_node_buf[2048]; - size_t scan_node_count; - pthread_mutex_t _lock; -}; - -#endif // FLASHGO_H From d15251565d3a48d3938eb7de800c0c24b98f85b2 Mon Sep 17 00:00:00 2001 From: EAIBOT Date: Mon, 7 Nov 2016 16:44:34 +0800 Subject: [PATCH 4/4] no message --- src/flashgo_node.cpp | 65 ++++++++++++++++++++++---------------------- 1 file changed, 32 insertions(+), 33 deletions(-) diff --git a/src/flashgo_node.cpp b/src/flashgo_node.cpp index dd9b545..9d9b339 100644 --- a/src/flashgo_node.cpp +++ b/src/flashgo_node.cpp @@ -129,47 +129,45 @@ int main(int argc, char * argv[]) { op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); if (op_result == -1) { fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); - //flashgo::~flashgo(); return -1; }else{ int isEAI = drv->getEAI(); if(isEAI != 0){ - drv->disconnect(); - if(115200 == (u_int32_t)serial_baudrate){ - drv->setScanDataProtocol(0); - serial_baudrate=230400; - op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); - if (op_result == -1) { - fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); - return -1; - }else{ - isEAI = drv->getEAI(); - if(isEAI != 0){ - fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); - return -1; - } - } - }else{ - drv->setScanDataProtocol(1); - serial_baudrate=115200; - op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); - if (op_result == -1) { - fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); - return -1; - }else{ - isEAI = drv->getEAI(); - if(isEAI != 0){ - fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); - return -1; - } - } - } + drv->disconnect(); + if(115200 == (u_int32_t)serial_baudrate){ + drv->setScanDataProtocol(0); + serial_baudrate=230400; + op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); + if (op_result == -1) { + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + }else{ + isEAI = drv->getEAI(); + if(isEAI != 0){ + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + } + } + }else{ + drv->setScanDataProtocol(1); + serial_baudrate=115200; + op_result = drv->connect(serial_port.c_str(), (u_int32_t)serial_baudrate); + if (op_result == -1) { + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + }else{ + isEAI = drv->getEAI(); + if(isEAI != 0){ + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); + return -1; + } + } + } } } // check health... if (!checkFlashLidarHealth(drv)) { - //flashgo::~flashgo(); return -1; } @@ -179,6 +177,7 @@ int main(int argc, char * argv[]) { ros::Time start_scan_time; ros::Time end_scan_time; double scan_duration; + ros::Rate rate(60); while (ros::ok()) { node_info nodes[360*2]; @@ -246,7 +245,7 @@ int main(int argc, char * argv[]) { frame_id); } } - + rate.sleep(); ros::spinOnce(); }