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;
+}