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_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..9d9b339 --- /dev/null +++ b/src/flashgo_node.cpp @@ -0,0 +1,255 @@ + +/* + * 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()); + 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)) { + return -1; + } + + // start scan... + drv->startScan(); + + 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]; + 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); + } + } + rate.sleep(); + ros::spinOnce(); + } + + // done! + drv->disconnect(); + return 0; +}