diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..1fcb842 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,42 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rplidar_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +1.5.7 (2016-12-15) +------------------ +* Release 1.5.7. +* Update RPLIDAR SDK to 1.5.7 +* Fixed the motor default speed at 10 HZ. Extend the measurement of max_distance from 6m to 8m. +* Contributors: kint + +1.5.5 (2016-08-23) +------------------ +* Release 1.5.5. +* Update RPLIDAR SDK to 1.5.5 +* Add RPLIDAR information print, and fix the standard motor speed of RPLIDAR A2. +* Contributors: kint + +1.5.4 (2016-06-02) +------------------ +* Release 1.5.4. +* Update RPLIDAR SDK to 1.5.4 +* Support RPLIDAR A2 +* Contributors: kint + +1.5.2 (2016-04-29) +------------------ +* Release 1.5.2. +* Update RPLIDAR SDK to 1.5.2 +* Support RPLIDAR A2 +* Contributors: kint + +1.0.1 (2014-06-03) +------------------ +* Release 1.0.1. +* Add angle compensate mechanism to compatible with ROS scan message +* Add RPLIDAR sdk to the repo. +* First release of RPLIDAR ROS package (1.0.0) +* Initial commit +* Contributors: Ling, RoboPeak Public Repos diff --git a/CMakeLists.txt b/CMakeLists.txt index 804f5a2..a5f2248 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS include_directories( ${RPLIDAR_SDK_PATH}/include ${RPLIDAR_SDK_PATH}/src + ${catkin_INCLUDE_DIRS} ) catkin_package() @@ -27,3 +28,14 @@ target_link_libraries(rplidarNode ${catkin_LIBRARIES}) add_executable(rplidarNodeClient src/client.cpp) target_link_libraries(rplidarNodeClient ${catkin_LIBRARIES}) + +install(TARGETS rplidarNode rplidarNodeClient + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch rviz sdk + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/LICENSE b/LICENSE index 75c90ff..af61f60 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,5 @@ -Copyright (c) 2014, RoboPeak Public Repos +Copyright (c) 2009 - 2014 RoboPeak Team +Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. All rights reserved. Redistribution and use in source and binary forms, with or without @@ -20,4 +21,4 @@ DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md index 548c6b8..ef46c26 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,13 @@ RPLIDAR ROS package ROS node and test application for RPLIDAR -Visit RoboPeak Website for more details about RPLIDAR. +Visit following Website for more details about RPLIDAR: + +rplidar roswiki: http://wiki.ros.org/rplidar + +rplidar HomePage: http://www.slamtec.com/en/Lidar + +rplidar Tutorial: https://github.com/robopeak/rplidar_ros/wiki How to build rplidar ros package ===================================================================== @@ -27,3 +33,8 @@ roslaunch rplidar_ros rplidar.launch rosrun rplidar_ros rplidarNodeClient You should see rplidar's scan result in the console + +RPLidar frame +===================================================================== +RPLidar frame must be broadcasted according to picture shown in +rplidar-frame.png diff --git a/launch/gmapping.launch b/launch/gmapping.launch new file mode 100755 index 0000000..e26f90e --- /dev/null +++ b/launch/gmapping.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/hectormapping.launch b/launch/hectormapping.launch new file mode 100755 index 0000000..3e29d84 --- /dev/null +++ b/launch/hectormapping.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/karto.launch b/launch/karto.launch new file mode 100755 index 0000000..20fdb8e --- /dev/null +++ b/launch/karto.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + diff --git a/launch/karto_mapper_params.yaml b/launch/karto_mapper_params.yaml new file mode 100644 index 0000000..e8467e0 --- /dev/null +++ b/launch/karto_mapper_params.yaml @@ -0,0 +1,37 @@ +# General Parameters +use_scan_matching: true +use_scan_barycenter: true +minimum_travel_distance: 0.3 +minimum_travel_heading: 0.4 # 0.2 #in radians + +scan_buffer_size: 67 +scan_buffer_maximum_scan_distance: 20.0 +link_match_minimum_response_fine: 0.6 +link_scan_maximum_distance: 4 # 6 + +do_loop_closing: true +loop_match_minimum_chain_size: 5 +loop_match_maximum_variance_coarse: 0.4 # gets squared later +loop_match_minimum_response_coarse: 0.4 # 0.6 +loop_match_minimum_response_fine: 0.6 + +# Correlation Parameters - Correlation Parameters +correlation_search_space_dimension: 2 +correlation_search_space_resolution: 0.05 +correlation_search_space_smear_deviation: 0.05 + +# Correlation Parameters - Loop Closure Parameters +loop_search_space_dimension: 10 # 2.8 +loop_search_space_resolution: 0.1 +loop_search_space_smear_deviation: 0.05 +loop_search_maximum_distance: 4.0 + +# Scan Matcher Parameters +distance_variance_penalty: 0.3 # gets squared later +angle_variance_penalty: 0.35 # in degrees (gets converted to radians then squared) +fine_search_angle_offset: 0.00349 # in degrees (gets converted to radians) +coarse_search_angle_offset: 0.349 # in degrees (gets converted to radians) +coarse_angle_resolution: 0.0349 # in degrees (gets converted to radians) +minimum_angle_penalty: 0.9 +minimum_distance_penalty: 0.5 +use_response_expansion: false diff --git a/launch/test_rplidar.launch b/launch/test_rplidar.launch new file mode 100644 index 0000000..7c65593 --- /dev/null +++ b/launch/test_rplidar.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/launch/view_gmapping.launch b/launch/view_gmapping.launch new file mode 100644 index 0000000..3b9163d --- /dev/null +++ b/launch/view_gmapping.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/launch/view_hectorSlam.launch b/launch/view_hectorSlam.launch new file mode 100644 index 0000000..7659814 --- /dev/null +++ b/launch/view_hectorSlam.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/launch/view_karto.launch b/launch/view_karto.launch new file mode 100644 index 0000000..7b25d4f --- /dev/null +++ b/launch/view_karto.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/launch/view_slam.launch b/launch/view_slam.launch new file mode 100644 index 0000000..7659814 --- /dev/null +++ b/launch/view_slam.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/package.xml b/package.xml index 1c6b141..015241a 100644 --- a/package.xml +++ b/package.xml @@ -1,18 +1,20 @@ rplidar_ros - 1.0.0 - The rplidar ros package - - Ling + 1.5.7 + The rplidar ros package, support rplidar A2/A1 + Slamtec ROS Maintainer BSD catkin roscpp rosconsole sensor_msgs + std_srvs roscpp rosconsole sensor_msgs + std_srvs + diff --git a/rplidar_A1.png b/rplidar_A1.png new file mode 100644 index 0000000..994def4 Binary files /dev/null and b/rplidar_A1.png differ diff --git a/rplidar_A2.png b/rplidar_A2.png new file mode 100644 index 0000000..3ca9294 Binary files /dev/null and b/rplidar_A2.png differ diff --git a/rviz/slam.rviz b/rviz/slam.rviz new file mode 100644 index 0000000..fa3ce00 --- /dev/null +++ b/rviz/slam.rviz @@ -0,0 +1,141 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RPLidarLaserScan1 + 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: RPLidarLaserScan +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: RPLidarLaserScan + 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 + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + 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 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.0344972 + Y: 0.065886 + Z: 0.148092 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.5698 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.66358 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 626 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001950000022cfc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000019b000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000022c000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003240000022c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1215 + X: 503 + Y: 227 diff --git a/scripts/create_udev_rules.sh b/scripts/create_udev_rules.sh new file mode 100755 index 0000000..ff260a9 --- /dev/null +++ b/scripts/create_udev_rules.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +echo "remap the device serial port(ttyUSBX) to rplidar" +echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB" +echo "start copy rplidar.rules to /etc/udev/rules.d/" +echo "`rospack find rplidar_ros`/scripts/rplidar.rules" +sudo cp `rospack find rplidar_ros`/scripts/rplidar.rules /etc/udev/rules.d +echo " " +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart +echo "finish " diff --git a/scripts/delete_udev_rules.sh b/scripts/delete_udev_rules.sh new file mode 100755 index 0000000..05c77f8 --- /dev/null +++ b/scripts/delete_udev_rules.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +echo "delete remap the device serial port(ttyUSBX) to rplidar" +echo "sudo rm /etc/udev/rules.d/rplidar.rules" +sudo rm /etc/udev/rules.d/rplidar.rules +echo " " +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart +echo "finish delete" diff --git a/scripts/rplidar.rules b/scripts/rplidar.rules new file mode 100644 index 0000000..d75ed81 --- /dev/null +++ b/scripts/rplidar.rules @@ -0,0 +1,4 @@ +# set the udev rule , make the device_port be fixed by rplidar +# +KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar" + diff --git a/sdk/README.txt b/sdk/README.txt index 48f0018..7853a0e 100644 --- a/sdk/README.txt +++ b/sdk/README.txt @@ -1,4 +1,5 @@ -Copyright (c) 2014, RoboPeak Public Repos +Copyright (c) 2009 - 2014 RoboPeak Team +Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. All rights reserved. Redistribution and use in source and binary forms, with or without @@ -23,8 +24,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + This folder contains RPLIDAR SDK source code which is provided by RoboPeak. -RoboPeak Website: http://www.robopeak.com -RPLIDAR_SDK_VERSION: 1.4.3 +RoboPeak Website: http://www.robopeak.com +SlamTec HomePage: http://www.slamtec.com +RPLIDAR_SDK_VERSION: 1.5.7 Note: The SDK version may not up-to-date. +rplidar product: http://www.slamtec.com/en/Lidar diff --git a/sdk/include/rplidar.h b/sdk/include/rplidar.h index be812c2..4f54714 100644 --- a/sdk/include/rplidar.h +++ b/sdk/include/rplidar.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * SDK All-in-one Header - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once @@ -41,3 +39,5 @@ #include "rplidar_cmd.h" #include "rplidar_driver.h" + +#define RPLIDAR_SDK_VERSION "1.5.7" diff --git a/sdk/include/rplidar_cmd.h b/sdk/include/rplidar_cmd.h index 1463770..4d30d20 100644 --- a/sdk/include/rplidar_cmd.h +++ b/sdk/include/rplidar_cmd.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,15 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Data Packet IO packet definition for RP-LIDAR - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ - #pragma once @@ -50,21 +47,61 @@ // Commands without payload but have response -#define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 -#define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 +#define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 +#define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 + +#define RPLIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17 + +// Commands with payload and have response +#define RPLIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 + +//add for A2 to set RPLIDAR motor pwm when using accessory board +#define RPLIDAR_CMD_SET_MOTOR_PWM 0xF0 +#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF #if defined(_WIN32) #pragma pack(1) #endif -// Response +// Payloads // ------------------------------------------ -#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81 +#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL 0 +#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE 1 +typedef struct _rplidar_payload_express_scan_t { + _u8 working_mode; + _u32 reserved; +} __attribute__((packed)) rplidar_payload_express_scan_t; + +#define MAX_MOTOR_PWM 1023 +#define DEFAULT_MOTOR_PWM 660 +typedef struct _rplidar_payload_motor_pwm_t { + _u16 pwm_value; +} __attribute__((packed)) rplidar_payload_motor_pwm_t; + +typedef struct _rplidar_payload_acc_board_flag_t { + _u32 reserved; +} __attribute__((packed)) rplidar_payload_acc_board_flag_t; +// Response +// ------------------------------------------ #define RPLIDAR_ANS_TYPE_DEVINFO 0x4 #define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6 +#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81 +// Added in FW ver 1.17 +#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82 + +// Added in FW ver 1.17 +#define RPLIDAR_ANS_TYPE_SAMPLE_RATE 0x15 + +#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF + +#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1) +typedef struct _rplidar_response_acc_board_flag_t { + _u32 support_flag; +} __attribute__((packed)) rplidar_response_acc_board_flag_t; + #define RPLIDAR_STATUS_OK 0x0 #define RPLIDAR_STATUS_WARNING 0x1 @@ -75,12 +112,40 @@ #define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) #define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 +typedef struct _rplidar_response_sample_rate_t { + _u16 std_sample_duration_us; + _u16 express_sample_duration_us; +} __attribute__((packed)) rplidar_response_sample_rate_t; + typedef struct _rplidar_response_measurement_node_t { _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; _u16 distance_q2; } __attribute__((packed)) rplidar_response_measurement_node_t; +//[distance_sync flags] +#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3) +#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC) + +typedef struct _rplidar_response_cabin_nodes_t { + _u16 distance_angle_1; // see [distance_sync flags] + _u16 distance_angle_2; // see [distance_sync flags] + _u8 offset_angles_q3; +} __attribute__((packed)) rplidar_response_cabin_nodes_t; + + +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5 + +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15) + +typedef struct _rplidar_response_capsule_measurement_nodes_t { + _u8 s_checksum_1; // see [s_checksum_1] + _u8 s_checksum_2; // see [s_checksum_1] + _u16 start_angle_sync_q6; + rplidar_response_cabin_nodes_t cabins[16]; +} __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t; + typedef struct _rplidar_response_device_info_t { _u8 model; _u16 firmware_version; diff --git a/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h index 5a6759f..a5af17a 100644 --- a/sdk/include/rplidar_driver.h +++ b/sdk/include/rplidar_driver.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Driver Interface - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once @@ -106,27 +104,64 @@ class RPlidarDriver { /// \param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0; + /// Get the sample duration information of the RPLIDAR. + /// + /// \param rateInfo The sample duration information returned from the RPLIDAR + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only. + /// + /// \param pwm The motor pwm value would like to set + virtual u_result setMotorPWM(_u16 pwm) = 0; + + /// Start RPLIDAR's motor when using accessory board + virtual u_result startMotor() = 0; + + /// Stop RPLIDAR's motor when using accessory board + virtual u_result stopMotor() = 0; + + /// Check whether the device support motor control. + /// Note: this API will disable grab. + /// + /// \param support Return the result. + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0; /// Calcuate RPLIDAR's current scanning frequency from the given scan data /// Please refer to the application note doc for details /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data /// - /// \param nodebuffer The buffer belongs to a 360degress scan data + /// \param inExpressMode Indicate whether the RPLIDAR is in express mode /// /// \param count The number of sample nodes inside the given buffer /// - // \param frequency The scanning frequency (in HZ) calcuated by the interface. - virtual u_result getFrequency(rplidar_response_measurement_node_t * nodebuffer, size_t count, float & frequency) = 0; + /// \param frequency The scanning frequency (in HZ) calcuated by the interface. + /// + /// \param is4kmode Return whether the RPLIDAR is working on 4k sample rate mode. + virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) = 0; - /// Ask the RPLIDAR core system to enter the scan mode + /// Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode) /// A background thread will be created by the RPLIDAR driver to fetch the scan data continuously. /// User Application can use the grabScanData() interface to retrieved the scan data cached previous by this background thread. /// /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. /// - /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result startScan(bool force = false, _u32 timeout = DEFAULT_TIMEOUT) = 0; + /// \param autoExpressMode Force the core system to trying express mode first, if the system does not support express mode, it will use normal mode. + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + virtual u_result startScan(bool force = false, bool autoExpressMode = true) = 0; + virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0; + virtual u_result startScanExpress(bool fixedAngle, _u32 timeout = DEFAULT_TIMEOUT) = 0; + /// Check whether the device support express mode. + /// + /// \param support Return the result. + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0; /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated /// diff --git a/sdk/include/rplidar_protocol.h b/sdk/include/rplidar_protocol.h index 48d1e93..e347bec 100644 --- a/sdk/include/rplidar_protocol.h +++ b/sdk/include/rplidar_protocol.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,15 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Data Packet IO protocol definition for RP-LIDAR - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ - #pragma once @@ -48,6 +45,8 @@ #define RPLIDAR_ANS_PKTFLAG_LOOP 0x1 +#define RPLIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF +#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) #if defined(_WIN32) #pragma pack(1) @@ -64,8 +63,7 @@ typedef struct _rplidar_cmd_packet_t { typedef struct _rplidar_ans_header_t { _u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1 _u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2 - _u32 size:30; - _u32 subType:2; + _u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; _u8 type; } __attribute__((packed)) rplidar_ans_header_t; diff --git a/sdk/include/rptypes.h b/sdk/include/rptypes.h index f12eab0..bc36bd9 100644 --- a/sdk/include/rptypes.h +++ b/sdk/include/rptypes.h @@ -1,36 +1,36 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES - * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT - * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT - * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR - * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Common Types definition - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once @@ -113,4 +113,4 @@ typedef uint32_t u_result; #define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) #define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) -typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); \ No newline at end of file +typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); diff --git a/sdk/src/arch/linux/arch_linux.h b/sdk/src/arch/linux/arch_linux.h index b6ffef2..04e2467 100644 --- a/sdk/src/arch/linux/arch_linux.h +++ b/sdk/src/arch/linux/arch_linux.h @@ -1,38 +1,36 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * - * Redistribution and use in source and binary forms, with or without + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Linux definition and includes - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once diff --git a/sdk/src/arch/linux/net_serial.cpp b/sdk/src/arch/linux/net_serial.cpp index cbe0ef9..49b8444 100644 --- a/sdk/src/arch/linux/net_serial.cpp +++ b/sdk/src/arch/linux/net_serial.cpp @@ -1,38 +1,36 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * - * Redistribution and use in source and binary forms, with or without + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Serial Device Driver for Win32 - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #include "arch/linux/arch_linux.h" #include "arch/linux/net_serial.h" @@ -124,11 +122,11 @@ bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) return false; } + _is_serial_opened = true; + //Clear the DTR bit to let the motor spin - uint32_t controll = TIOCM_DTR; - ioctl(serial_fd, TIOCMBIC, &controll); + clearDTR(); - _is_serial_opened = true; return true; } @@ -272,6 +270,21 @@ size_t raw_serial::rxqueue_count() return remaining; } +void raw_serial::setDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIS, &dtr_bit); +} + +void raw_serial::clearDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIC, &dtr_bit); +} void raw_serial::_init() { diff --git a/sdk/src/arch/linux/net_serial.h b/sdk/src/arch/linux/net_serial.h index 65d739c..ffaf32a 100644 --- a/sdk/src/arch/linux/net_serial.h +++ b/sdk/src/arch/linux/net_serial.h @@ -1,38 +1,36 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * - * Redistribution and use in source and binary forms, with or without + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Serial Device Driver for Win32 - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once @@ -50,7 +48,7 @@ class raw_serial : public rp::hal::serial_rxtx raw_serial(); virtual ~raw_serial(); - virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = NULL); + virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0); virtual bool open(); virtual void close(); virtual void flush( _u32 flags); @@ -65,9 +63,12 @@ class raw_serial : public rp::hal::serial_rxtx virtual size_t rxqueue_count(); + virtual void setDTR(); + virtual void clearDTR(); + _u32 getTermBaudBitmap(_u32 baud); protected: - bool open(const char * portname, uint32_t baudrate, uint32_t flags = NULL); + bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); void _init(); char _portName[200]; diff --git a/sdk/src/arch/linux/thread.hpp b/sdk/src/arch/linux/thread.hpp index 847157e..ee8393e 100644 --- a/sdk/src/arch/linux/thread.hpp +++ b/sdk/src/arch/linux/thread.hpp @@ -1,38 +1,36 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * - * Redistribution and use in source and binary forms, with or without + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Thread abstract layer implementation - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #include "arch/linux/arch_linux.h" diff --git a/sdk/src/arch/linux/timer.cpp b/sdk/src/arch/linux/timer.cpp index 1192ed1..12929b1 100644 --- a/sdk/src/arch/linux/timer.cpp +++ b/sdk/src/arch/linux/timer.cpp @@ -1,38 +1,36 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * - * Redistribution and use in source and binary forms, with or without + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * High Resolution Timer Impl on Linux - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #include "arch/linux/arch_linux.h" diff --git a/sdk/src/arch/linux/timer.h b/sdk/src/arch/linux/timer.h index d872612..4d7015f 100644 --- a/sdk/src/arch/linux/timer.h +++ b/sdk/src/arch/linux/timer.h @@ -1,43 +1,51 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * - * Redistribution and use in source and binary forms, with or without + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * High Resolution Timer Impl on Linux - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once #include "rptypes.h" +#include +static inline void delay(_word_size_t ms){ + while (ms>=1000){ + usleep(1000*1000); + ms-=1000; + }; + if (ms!=0) + usleep(ms*1000); +} + // TODO: the highest timer interface should be clock_gettime namespace rp{ namespace arch{ diff --git a/sdk/src/arch/macOS/arch_macOS.h b/sdk/src/arch/macOS/arch_macOS.h new file mode 100644 index 0000000..e69f059 --- /dev/null +++ b/sdk/src/arch/macOS/arch_macOS.h @@ -0,0 +1,66 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +// libc dep +#include +#include +#include +#include +#include +#include +#include +#include + +// libc++ dep +#include +#include + + +// POSIX specific +extern "C" { +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +#include "arch/macOS/timer.h" + diff --git a/sdk/src/arch/macOS/net_serial.cpp b/sdk/src/arch/macOS/net_serial.cpp new file mode 100644 index 0000000..701641e --- /dev/null +++ b/sdk/src/arch/macOS/net_serial.cpp @@ -0,0 +1,347 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/macOS/arch_macOS.h" +#include "arch/macOS/net_serial.h" +#include +#include + +namespace rp{ namespace arch{ namespace net{ + +raw_serial::raw_serial() + : rp::hal::serial_rxtx() + , _baudrate(0) + , _flags(0) + , serial_fd(-1) +{ + _init(); +} + +raw_serial::~raw_serial() +{ + close(); + +} + +bool raw_serial::open() +{ + return open(_portName, _baudrate, _flags); +} + +bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags) +{ + strncpy(_portName, portname, sizeof(_portName)); + _baudrate = baudrate; + _flags = flags; + return true; +} + +bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) +{ + if (isOpened()) close(); + + serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY); + + if (serial_fd == -1) return false; + + struct termios options, oldopt; + tcgetattr(serial_fd, &oldopt); + bzero(&options,sizeof(struct termios)); + + _u32 termbaud = getTermBaudBitmap(baudrate); + + if (termbaud == (_u32)-1) { + close(); + return false; + } + cfsetispeed(&options, termbaud); + cfsetospeed(&options, termbaud); + + // enable rx and tx + options.c_cflag |= (CLOCAL | CREAD); + + + options.c_cflag &= ~PARENB; //no checkbit + options.c_cflag &= ~CSTOPB; //1bit stop bit + + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; /* Select 8 data bits */ + +#ifdef CNEW_RTSCTS + options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control +#endif + + options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control + + // raw input mode + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + // raw output mode + options.c_oflag &= ~OPOST; + + tcflush(serial_fd,TCIFLUSH); +/* + if (fcntl(serial_fd, F_SETFL, FNDELAY)) + { + close(); + return false; + } +*/ + if (tcsetattr(serial_fd, TCSANOW, &options)) + { + close(); + return false; + } + + _is_serial_opened = true; + + //Clear the DTR bit to let the motor spin + clearDTR(); + + return true; +} + +void raw_serial::close() +{ + if (serial_fd != -1) + ::close(serial_fd); + serial_fd = -1; + + _is_serial_opened = false; +} + +int raw_serial::senddata(const unsigned char * data, _word_size_t size) +{ +// FIXME: non-block io should be used + if (!isOpened()) 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= data_count) + { + return 0; + } + } + + while ( isOpened() ) + { + /* Do the select */ + int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val); + + if (n < 0) + { + // select error + return ANS_DEV_ERR; + } + else if (n == 0) + { + // time out + return ANS_TIMEOUT; + } + else + { + // data avaliable + assert (FD_ISSET(serial_fd, &input_set)); + + + if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR; + 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 ANS_DEV_ERR; +} + +size_t raw_serial::rxqueue_count() +{ + if ( !isOpened() ) return 0; + size_t remaining; + + if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0; + return remaining; +} + +void raw_serial::setDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIS, &dtr_bit); +} + +void raw_serial::clearDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIC, &dtr_bit); +} + +void raw_serial::_init() +{ + serial_fd = 0; + _portName[0] = 0; + required_tx_cnt = required_rx_cnt = 0; +} + + + +_u32 raw_serial::getTermBaudBitmap(_u32 baud) +{ +#define BAUD_CONV(_baud_) case _baud_: return _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; +} + +}}} //end rp::arch::net + + + +//begin rp::hal +namespace rp{ namespace hal{ + + serial_rxtx * serial_rxtx::CreateRxTx() + { + return new rp::arch::net::raw_serial(); + } + + void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx) + { + delete rxtx; + } + +}} //end rp::hal diff --git a/sdk/src/arch/macOS/net_serial.h b/sdk/src/arch/macOS/net_serial.h new file mode 100644 index 0000000..e7554a3 --- /dev/null +++ b/sdk/src/arch/macOS/net_serial.h @@ -0,0 +1,84 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/abs_rxtx.h" + +namespace rp{ namespace arch{ namespace net{ + +class raw_serial : public rp::hal::serial_rxtx +{ +public: + enum{ + SERIAL_RX_BUFFER_SIZE = 512, + SERIAL_TX_BUFFER_SIZE = 128, + }; + + raw_serial(); + virtual ~raw_serial(); + virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0); + virtual bool open(); + virtual void close(); + virtual void flush( _u32 flags); + + virtual int waitfordata(_word_size_t data_count,_u32 timeout = -1, _word_size_t * returned_size = NULL); + + virtual int senddata(const unsigned char * data, _word_size_t size); + virtual int recvdata(unsigned char * data, _word_size_t size); + + virtual int waitforsent(_u32 timeout = -1, _word_size_t * returned_size = NULL); + virtual int waitforrecv(_u32 timeout = -1, _word_size_t * returned_size = NULL); + + virtual size_t rxqueue_count(); + + virtual void setDTR(); + virtual void clearDTR(); + + _u32 getTermBaudBitmap(_u32 baud); +protected: + bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); + void _init(); + + char _portName[200]; + uint32_t _baudrate; + uint32_t _flags; + + int serial_fd; + + size_t required_tx_cnt; + size_t required_rx_cnt; +}; + +}}} diff --git a/sdk/src/arch/macOS/thread.hpp b/sdk/src/arch/macOS/thread.hpp new file mode 100644 index 0000000..5d908ae --- /dev/null +++ b/sdk/src/arch/macOS/thread.hpp @@ -0,0 +1,79 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/macOS/arch_macOS.h" + +namespace rp{ namespace hal{ + +Thread Thread::create(thread_proc_t proc, void * data) +{ + Thread newborn(proc, data); + + // tricky code, we assume pthread_t is not a structure but a word size value + assert( sizeof(newborn._handle) >= sizeof(pthread_t)); + + pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data); + + return newborn; +} + +u_result Thread::terminate() +{ + if (!this->_handle) return RESULT_OK; + + // return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; + return RESULT_OK; +} + +u_result Thread::setPriority( priority_val_t p) +{ + if (!this->_handle) return RESULT_OPERATION_FAIL; + // simply ignore this request + return RESULT_OK; +} + +Thread::priority_val_t Thread::getPriority() +{ + return PRIORITY_NORMAL; +} + +u_result Thread::join(unsigned long timeout) +{ + if (!this->_handle) return RESULT_OK; + + pthread_join((pthread_t)(this->_handle), NULL); + return RESULT_OK; +} + +}} diff --git a/sdk/src/arch/macOS/timer.cpp b/sdk/src/arch/macOS/timer.cpp new file mode 100644 index 0000000..a000744 --- /dev/null +++ b/sdk/src/arch/macOS/timer.cpp @@ -0,0 +1,53 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/macOS/arch_macOS.h" + + +namespace rp{ namespace arch{ +_u64 getus() +{ + timeval now; + gettimeofday(&now,NULL); + return now.tv_sec*1000000 + now.tv_usec; +} + +_u32 rp_getms() +{ + timeval now; + gettimeofday(&now,NULL); + return now.tv_sec*1000L + now.tv_usec/1000L; +} + +}} diff --git a/sdk/src/arch/macOS/timer.h b/sdk/src/arch/macOS/timer.h new file mode 100644 index 0000000..4d7015f --- /dev/null +++ b/sdk/src/arch/macOS/timer.h @@ -0,0 +1,57 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "rptypes.h" + +#include +static inline void delay(_word_size_t ms){ + while (ms>=1000){ + usleep(1000*1000); + ms-=1000; + }; + if (ms!=0) + usleep(ms*1000); +} + +// TODO: the highest timer interface should be clock_gettime +namespace rp{ namespace arch{ + +_u64 rp_getus(); +_u32 rp_getms(); + +}} + +#define getms() rp::arch::rp_getms() diff --git a/sdk/src/arch/win32/arch_win32.h b/sdk/src/arch/win32/arch_win32.h index 14e6ad2..94e9807 100644 --- a/sdk/src/arch/win32/arch_win32.h +++ b/sdk/src/arch/win32/arch_win32.h @@ -1,38 +1,36 @@ -/* - * Copyright (c) 2014, RoboPeak - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ -/* - * RoboPeak LIDAR System - * Win32 definition and includes - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ #pragma once diff --git a/sdk/src/arch/win32/net_serial.cpp b/sdk/src/arch/win32/net_serial.cpp index 605d5f9..6eee344 100644 --- a/sdk/src/arch/win32/net_serial.cpp +++ b/sdk/src/arch/win32/net_serial.cpp @@ -1,39 +1,37 @@ -/* - * Copyright (c) 2014, RoboPeak - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ -/* - * RoboPeak LIDAR System - * Serial Device Driver for Win32 - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ - +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + #include "sdkcommon.h" #include "net_serial.h" @@ -104,9 +102,6 @@ bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags) return false; } - //Clear the DTR bit to let the motor spin - EscapeCommFunction(_serial_handle, CLRDTR); - if (!SetCommTimeouts(_serial_handle, &_co)) { close(); @@ -127,6 +122,10 @@ bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags) Sleep(30); _is_serial_opened = true; + + //Clear the DTR bit set DTR=high + clearDTR(); + return true; } @@ -230,7 +229,9 @@ int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_s COMSTAT stat; DWORD error; DWORD msk,length; - if (returned_size==NULL) returned_size=(size_t *)&length; + size_t dummy_length; + + if (returned_size==NULL) returned_size=(size_t *)&dummy_length; if ( isOpened()) { @@ -299,6 +300,20 @@ size_t raw_serial::rxqueue_count() return com_stat.cbInQue; } +void raw_serial::setDTR() +{ + if ( !isOpened() ) return; + + EscapeCommFunction(_serial_handle, SETDTR); +} + +void raw_serial::clearDTR() +{ + if ( !isOpened() ) return; + + EscapeCommFunction(_serial_handle, CLRDTR); +} + void raw_serial::_init() { diff --git a/sdk/src/arch/win32/net_serial.h b/sdk/src/arch/win32/net_serial.h index 42fed59..ddec5c7 100644 --- a/sdk/src/arch/win32/net_serial.h +++ b/sdk/src/arch/win32/net_serial.h @@ -1,38 +1,36 @@ -/* - * Copyright (c) 2014, RoboPeak - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ -/* - * RoboPeak LIDAR System - * Serial Device Driver for Win32 - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ #pragma once @@ -67,6 +65,8 @@ class raw_serial : public rp::hal::serial_rxtx virtual size_t rxqueue_count(); + virtual void setDTR(); + virtual void clearDTR(); protected: bool open(const char * portname, _u32 baudrate, _u32 flags); diff --git a/sdk/src/arch/win32/timer.cpp b/sdk/src/arch/win32/timer.cpp index e8b60bf..f690e94 100644 --- a/sdk/src/arch/win32/timer.cpp +++ b/sdk/src/arch/win32/timer.cpp @@ -1,38 +1,36 @@ -/* - * Copyright (c) 2014, RoboPeak - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ -/* - * RoboPeak LIDAR System - * High Resolution Timer Impl on Win32 - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ #include "sdkcommon.h" #include diff --git a/sdk/src/arch/win32/timer.h b/sdk/src/arch/win32/timer.h index 0c5ebd7..e074ecb 100644 --- a/sdk/src/arch/win32/timer.h +++ b/sdk/src/arch/win32/timer.h @@ -1,38 +1,36 @@ -/* - * Copyright (c) 2014, RoboPeak - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ -/* - * RoboPeak LIDAR System - * High Resolution Timer Impl on Win32 - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ #pragma once diff --git a/sdk/src/arch/win32/winthread.hpp b/sdk/src/arch/win32/winthread.hpp index 043691f..b2365e0 100644 --- a/sdk/src/arch/win32/winthread.hpp +++ b/sdk/src/arch/win32/winthread.hpp @@ -1,38 +1,36 @@ -/* - * Copyright (c) 2014, RoboPeak - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ -/* - * RoboPeak LIDAR System - * Thread abstract layer implementation - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ #include "sdkcommon.h" #include diff --git a/sdk/src/hal/abs_rxtx.h b/sdk/src/hal/abs_rxtx.h index 1be3258..a29e349 100644 --- a/sdk/src/hal/abs_rxtx.h +++ b/sdk/src/hal/abs_rxtx.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Serial Driver Interface - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once @@ -71,6 +69,9 @@ class serial_rxtx virtual size_t rxqueue_count() = 0; + virtual void setDTR() = 0; + virtual void clearDTR() = 0; + virtual bool isOpened() { return _is_serial_opened; diff --git a/sdk/src/hal/event.h b/sdk/src/hal/event.h index 43f9b55..f6464ad 100644 --- a/sdk/src/hal/event.h +++ b/sdk/src/hal/event.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,15 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Lock abstract layer - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ - #pragma once namespace rp{ namespace hal{ diff --git a/sdk/src/hal/locker.h b/sdk/src/hal/locker.h index 34b8286..8846571 100644 --- a/sdk/src/hal/locker.h +++ b/sdk/src/hal/locker.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Lock abstract layer - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once namespace rp{ namespace hal{ @@ -72,14 +70,21 @@ class Locker return LOCK_TIMEOUT; } +#else +#ifdef _MACOS + if (timeout !=0 ) { + if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; + } #else if (timeout == 0xFFFFFFFF){ if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; } +#endif else if (timeout == 0) { if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK; } +#ifndef _MACOS else { timespec wait_time; @@ -102,6 +107,7 @@ class Locker return LOCK_TIMEOUT; } } +#endif #endif return LOCK_FAILED; diff --git a/sdk/src/hal/thread.cpp b/sdk/src/hal/thread.cpp index d8672fe..2e5b34f 100644 --- a/sdk/src/hal/thread.cpp +++ b/sdk/src/hal/thread.cpp @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,20 +31,14 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Thread abstract layer - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #include "sdkcommon.h" #include "hal/thread.h" #if defined(_WIN32) #include "arch/win32/winthread.hpp" +#elif defined(_MACOS) +#include "arch/macOS/thread.hpp" #elif defined(__GNUC__) #include "arch/linux/thread.hpp" #else diff --git a/sdk/src/hal/thread.h b/sdk/src/hal/thread.h index b3deae0..eb867e4 100644 --- a/sdk/src/hal/thread.h +++ b/sdk/src/hal/thread.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Thread abstract layer - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once diff --git a/sdk/src/hal/util.h b/sdk/src/hal/util.h index 60939d7..d09ab00 100644 --- a/sdk/src/hal/util.h +++ b/sdk/src/hal/util.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Utility Functions - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp index f41ab5e..478a773 100644 --- a/sdk/src/rplidar_driver.cpp +++ b/sdk/src/rplidar_driver.cpp @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Serial based RPlidar Driver - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #include "sdkcommon.h" #include "hal/abs_rxtx.h" @@ -72,9 +70,12 @@ void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) RPlidarDriverSerialImpl::RPlidarDriverSerialImpl() : _isConnected(false) , _isScanning(false) + , _isSupportingMotorCtrl(false) { _rxtx = rp::hal::serial_rxtx::CreateRxTx(); _cached_scan_node_count = 0; + _cached_sampleduration_std = LEGACY_SAMPLE_DURATION; + _cached_sampleduration_express = LEGACY_SAMPLE_DURATION; } RPlidarDriverSerialImpl::~RPlidarDriverSerialImpl() @@ -87,19 +88,26 @@ RPlidarDriverSerialImpl::~RPlidarDriverSerialImpl() u_result RPlidarDriverSerialImpl::connect(const char * port_path, _u32 baudrate, _u32 flag) { - rp::hal::AutoLocker l(_lock); if (isConnected()) return RESULT_ALREADY_DONE; if (!_rxtx) return RESULT_INSUFFICIENT_MEMORY; - // establish the serial connection... - if (!_rxtx->bind(port_path, baudrate) || !_rxtx->open()) { - return RESULT_INVALID_DATA; + { + rp::hal::AutoLocker l(_lock); + + // establish the serial connection... + if (!_rxtx->bind(port_path, baudrate) || !_rxtx->open()) { + return RESULT_INVALID_DATA; + } + + _rxtx->flush(0); } - _rxtx->flush(0); _isConnected = true; + checkMotorCtrlSupport(_isSupportingMotorCtrl); + stopMotor(); + return RESULT_OK; } @@ -119,8 +127,13 @@ bool RPlidarDriverSerialImpl::isConnected() u_result RPlidarDriverSerialImpl::reset(_u32 timeout) { u_result ans; - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) { - return ans; + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) { + return ans; + } } return RESULT_OK; } @@ -135,6 +148,7 @@ u_result RPlidarDriverSerialImpl::getHealth(rplidar_response_device_health_t & h { rp::hal::AutoLocker l(_lock); + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH))) { return ans; } @@ -149,11 +163,12 @@ u_result RPlidarDriverSerialImpl::getHealth(rplidar_response_device_health_t & h return RESULT_INVALID_DATA; } - if (response_header.size < sizeof(rplidar_response_device_health_t)) { + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if ( header_size < sizeof(rplidar_response_device_health_t)) { return RESULT_INVALID_DATA; } - if (_rxtx->waitfordata(response_header.size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { return RESULT_OPERATION_TIMEOUT; } _rxtx->recvdata(reinterpret_cast<_u8 *>(&healthinfo), sizeof(healthinfo)); @@ -171,6 +186,7 @@ u_result RPlidarDriverSerialImpl::getDeviceInfo(rplidar_response_device_info_t & { rp::hal::AutoLocker l(_lock); + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO))) { return ans; } @@ -185,11 +201,12 @@ u_result RPlidarDriverSerialImpl::getDeviceInfo(rplidar_response_device_info_t & return RESULT_INVALID_DATA; } - if (response_header.size < sizeof(rplidar_response_device_info_t)) { + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(rplidar_response_device_info_t)) { return RESULT_INVALID_DATA; } - if (_rxtx->waitfordata(response_header.size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { return RESULT_OPERATION_TIMEOUT; } @@ -198,21 +215,21 @@ u_result RPlidarDriverSerialImpl::getDeviceInfo(rplidar_response_device_info_t & return RESULT_OK; } -u_result RPlidarDriverSerialImpl::getFrequency(rplidar_response_measurement_node_t * nodebuffer, size_t count, float & frequency) +u_result RPlidarDriverSerialImpl::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) { - const float RPLIDAR_SAMPLE_DURATION = 490.3; //ms - if (count == 0) - { - frequency = 0.0f; - } - else - { - frequency = (float)(1000000/(count * RPLIDAR_SAMPLE_DURATION)); - } + _u16 sample_duration = inExpressMode?_cached_sampleduration_express:_cached_sampleduration_std; + frequency = 1000000.0f/(count * sample_duration); + + if (sample_duration <= 277) { + is4kmode = true; + } else { + is4kmode = false; + } + return RESULT_OK; } -u_result RPlidarDriverSerialImpl::startScan(bool force, _u32 timeout) +u_result RPlidarDriverSerialImpl::startScanNormal(bool force, _u32 timeout) { u_result ans; if (!isConnected()) return RESULT_OPERATION_FAIL; @@ -238,21 +255,117 @@ u_result RPlidarDriverSerialImpl::startScan(bool force, _u32 timeout) return RESULT_INVALID_DATA; } - if (response_header.size < sizeof(rplidar_response_measurement_node_t)) { + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(rplidar_response_measurement_node_t)) { return RESULT_INVALID_DATA; } _isScanning = true; _cachethread = CLASS_THREAD(RPlidarDriverSerialImpl, _cacheScanData); + if (_cachethread.getHandle() == 0) { + return RESULT_OPERATION_FAIL; + } + } + return RESULT_OK; +} + +u_result RPlidarDriverSerialImpl::checkExpressScanSupported(bool & support, _u32 timeout) +{ + rplidar_response_device_info_t devinfo; + + support = false; + u_result ans = getDeviceInfo(devinfo, timeout); + + if (IS_FAIL(ans)) return ans; + + if (devinfo.firmware_version >= ((0x1<<8) | 17)) { + support = true; + rplidar_response_sample_rate_t sample_rate; + getSampleDuration_uS(sample_rate); + _cached_sampleduration_express = sample_rate.express_sample_duration_us; + _cached_sampleduration_std = sample_rate.std_sample_duration_us; + } + + return RESULT_OK; +} + +u_result RPlidarDriverSerialImpl::startScanExpress(bool fixedAngle, _u32 timeout) +{ + u_result ans; + if (!isConnected()) return RESULT_OPERATION_FAIL; + if (_isScanning) return RESULT_ALREADY_DONE; + + stop(); //force the previous operation to stop + + { + rp::hal::AutoLocker l(_lock); + + rplidar_payload_express_scan_t scanReq; + scanReq.working_mode = (fixedAngle?RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE:RPLIDAR_EXPRESS_SCAN_MODE_NORMAL); + scanReq.reserved = 0; + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_EXPRESS_SCAN,&scanReq, sizeof(scanReq)))) { + return ans; + } + + // waiting for confirmation + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { + return RESULT_INVALID_DATA; + } + + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverSerialImpl, _cacheCapsuledScanData); + if (_cachethread.getHandle() == 0) { + return RESULT_OPERATION_FAIL; + } } return RESULT_OK; } + +u_result RPlidarDriverSerialImpl::startScan(bool force, bool autoExpressMode) +{ + bool isExpressModeSupported; + u_result ans; + + if (autoExpressMode) { + ans = checkExpressScanSupported(isExpressModeSupported); + + if (IS_FAIL(ans)) return ans; + + if (isExpressModeSupported) { + return startScanExpress(false); + } + } + + return startScanNormal(force); +} + u_result RPlidarDriverSerialImpl::stop(_u32 timeout) { + u_result ans; _disableDataGrabbing(); - u_result ans = _sendCommand(RPLIDAR_CMD_STOP); - return ans; + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_STOP))) { + return ans; + } + } + + return RESULT_OK; } u_result RPlidarDriverSerialImpl::_cacheScanData() @@ -298,6 +411,116 @@ u_result RPlidarDriverSerialImpl::_cacheScanData() return RESULT_OK; } +void RPlidarDriverSerialImpl::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount) +{ + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF)<< 2); + int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360<<8); + } + + int angleInc_q16 = (diffAngle_q8 << 3); + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos) + { + int dist_q2[2]; + int angle_q6[2]; + int syncBit[2]; + + dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); + dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); + + int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4)); + int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4)); + + angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10); + syncBit[0] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; + currentAngle_raw_q16 += angleInc_q16; + + + angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10); + syncBit[1] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; + currentAngle_raw_q16 += angleInc_q16; + + for (int cpos = 0; cpos < 2; ++cpos) { + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6); + if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6); + + rplidar_response_measurement_node_t node; + + node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + if (dist_q2[cpos]) node.sync_quality |= (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + + node.angle_q6_checkbit = (1 | (angle_q6[cpos]<<1)); + node.distance_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + + } + } + + _cached_previous_capsuledata = capsule; + _is_previous_capsuledataRdy = true; +} + + +u_result RPlidarDriverSerialImpl::_cacheCapsuledScanData() +{ + rplidar_response_capsule_measurement_nodes_t capsule_node; + rplidar_response_measurement_node_t local_buf[128]; + size_t count = 128; + rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + u_result ans; + memset(local_scan, 0, sizeof(local_scan)); + + _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete + + while(_isScanning) + { + if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) { + if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { + _isScanning = false; + return RESULT_OPERATION_FAIL; + } else { + // current data is invalid, do not use it. + continue; + } + } + + _capsuleToNormal(capsule_node, local_buf, count); + + for (size_t pos = 0; pos < count; ++pos) + { + if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) + { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_t)); + _cached_scan_node_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow + } + } + _isScanning = false; + + return RESULT_OK; +} + u_result RPlidarDriverSerialImpl::grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) { switch (_dataEvt.wait(timeout)) @@ -328,8 +551,7 @@ u_result RPlidarDriverSerialImpl::grabScanData(rplidar_response_measurement_node u_result RPlidarDriverSerialImpl::ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) { float inc_origin_angle = 360.0/count; - rplidar_response_measurement_node_t *tmpbuffer = new rplidar_response_measurement_node_t[count]; - int i = 0; + size_t i = 0; //Tune head for (i = 0; i < count; i++) { @@ -368,7 +590,7 @@ u_result RPlidarDriverSerialImpl::ascendScanData(rplidar_response_measurement_no //Fill invalid angle in the scan float frontAngle = (nodebuffer[0].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - for (i = 0; i < count; i++) { + for (i = 1; i < 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; @@ -376,31 +598,18 @@ u_result RPlidarDriverSerialImpl::ascendScanData(rplidar_response_measurement_no nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; } } - - // find zero_position in the full scan - size_t zero_pos = 0; - float pre_degree = (nodebuffer[0].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - for (i = 1; i < count ; ++i) { - float degree = (nodebuffer[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - if (zero_pos == 0 && (pre_degree - degree > 180)) { - zero_pos = i; - break; + // Reorder the scan according to the angle value + for (i = 0; i < (count-1); i++){ + for (size_t j = (i+1); j < count; j++){ + if(nodebuffer[i].angle_q6_checkbit > nodebuffer[j].angle_q6_checkbit){ + rplidar_response_measurement_node_t temp = nodebuffer[i]; + nodebuffer[i] = nodebuffer[j]; + nodebuffer[j] = temp; + } } - pre_degree = degree; - } - - // reorder the scan - for (i = zero_pos; i < count; i++) { - tmpbuffer[i-zero_pos] = nodebuffer[i]; - } - for (i = 0; i < zero_pos; i++) { - tmpbuffer[i+count-zero_pos] = nodebuffer[i]; } - memcpy(nodebuffer, tmpbuffer, count*sizeof(rplidar_response_measurement_node_t)); - delete tmpbuffer; - return RESULT_OK; } @@ -462,6 +671,7 @@ u_result RPlidarDriverSerialImpl::_waitNode(rplidar_response_measurement_node_t return RESULT_OPERATION_TIMEOUT; } + u_result RPlidarDriverSerialImpl::_waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) { if (!_isConnected) { @@ -488,6 +698,89 @@ u_result RPlidarDriverSerialImpl::_waitScanData(rplidar_response_measurement_nod return RESULT_OPERATION_TIMEOUT; } + +u_result RPlidarDriverSerialImpl::_waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout) +{ + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)]; + _u8 *nodeBuffer = (_u8*)&node; + _u32 waitTime; + + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + int ans = _rxtx->waitfordata(remainSize, timeout-waitTime, &recvSize); + if (ans == rp::hal::serial_rxtx::ANS_DEV_ERR) + return RESULT_OPERATION_FAIL; + else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT) + return RESULT_OPERATION_TIMEOUT; + + if (recvSize > remainSize) recvSize = remainSize; + + _rxtx->recvdata(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + _u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (currentByte>>4); + if ( tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 ) { + // pass + } else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (currentByte>>4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } else { + recvPos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(rplidar_response_capsule_measurement_nodes_t)) { + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2<<4)); + for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= nodeBuffer[cpos]; + } + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + return RESULT_OK; + } + return RESULT_OK; + } + _is_previous_capsuledataRdy = false; + return RESULT_INVALID_DATA; + } + } + } + _is_previous_capsuledataRdy = false; + return RESULT_OPERATION_TIMEOUT; +} + + u_result RPlidarDriverSerialImpl::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) { _u8 pkt_header[10]; @@ -588,4 +881,146 @@ void RPlidarDriverSerialImpl::_disableDataGrabbing() _cachethread.join(); } +u_result RPlidarDriverSerialImpl::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout) +{ + if (!isConnected()) return RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + rplidar_response_device_info_t devinfo; + // 1. fetch the device version first... + u_result ans = getDeviceInfo(devinfo, timeout); + + rateInfo.express_sample_duration_us = _cached_sampleduration_express; + rateInfo.std_sample_duration_us = _cached_sampleduration_std; + + if (devinfo.firmware_version < ((0x1<<8) | 17)) { + // provide fake data... + + return RESULT_OK; + } + + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_SAMPLERATE))) { + return ans; + } + + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if ( header_size < sizeof(rplidar_response_sample_rate_t)) { + return RESULT_INVALID_DATA; + } + + if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + return RESULT_OPERATION_TIMEOUT; + } + _rxtx->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo)); + } + return RESULT_OK; +} + +u_result RPlidarDriverSerialImpl::checkMotorCtrlSupport(bool & support, _u32 timeout) +{ + u_result ans; + support = false; + + if (!isConnected()) return RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + + rplidar_payload_acc_board_flag_t flag; + flag.reserved = 0; + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag)))) { + return ans; + } + + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG) { + return RESULT_INVALID_DATA; + } + + _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); + if ( header_size < sizeof(rplidar_response_acc_board_flag_t)) { + return RESULT_INVALID_DATA; + } + + if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + return RESULT_OPERATION_TIMEOUT; + } + rplidar_response_acc_board_flag_t acc_board_flag; + _rxtx->recvdata(reinterpret_cast<_u8 *>(&acc_board_flag), sizeof(acc_board_flag)); + + if (acc_board_flag.support_flag & RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) { + support = true; + } + } + return RESULT_OK; +} + +u_result RPlidarDriverSerialImpl::setMotorPWM(_u16 pwm) +{ + u_result ans; + rplidar_payload_motor_pwm_t motor_pwm; + motor_pwm.pwm_value = pwm; + + { + rp::hal::AutoLocker l(_lock); + + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_SET_MOTOR_PWM,(const _u8 *)&motor_pwm, sizeof(motor_pwm)))) { + return ans; + } + } + + return RESULT_OK; +} + +u_result RPlidarDriverSerialImpl::startMotor() +{ + if (_isSupportingMotorCtrl) { // RPLIDAR A2 + setMotorPWM(DEFAULT_MOTOR_PWM); + delay(500); + return RESULT_OK; + } else { // RPLIDAR A1 + rp::hal::AutoLocker l(_lock); + _rxtx->clearDTR(); + delay(500); + return RESULT_OK; + } +} + +u_result RPlidarDriverSerialImpl::stopMotor() +{ + if (_isSupportingMotorCtrl) { // RPLIDAR A2 + setMotorPWM(0); + delay(500); + return RESULT_OK; + } else { // RPLIDAR A1 + rp::hal::AutoLocker l(_lock); + _rxtx->setDTR(); + delay(500); + return RESULT_OK; + } +} + }}} diff --git a/sdk/src/rplidar_driver_serial.h b/sdk/src/rplidar_driver_serial.h index 99afa4e..a2d12de 100644 --- a/sdk/src/rplidar_driver_serial.h +++ b/sdk/src/rplidar_driver_serial.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,14 +31,6 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * Serial based RPlidar Driver - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #pragma once @@ -46,6 +44,10 @@ class RPlidarDriverSerialImpl : public RPlidarDriver MAX_SCAN_NODES = 2048, }; + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + RPlidarDriverSerialImpl(); virtual ~RPlidarDriverSerialImpl(); @@ -58,33 +60,53 @@ class RPlidarDriverSerialImpl : public RPlidarDriver virtual u_result getHealth(rplidar_response_device_health_t &, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result getDeviceInfo(rplidar_response_device_info_t &, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result getFrequency(rplidar_response_measurement_node_t * nodebuffer, size_t count, float & frequency); + virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT); + + virtual u_result setMotorPWM(_u16 pwm); + virtual u_result startMotor(); + virtual u_result stopMotor(); + virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode); + + virtual u_result startScan(bool force = false, bool autoExpressMode = true); + virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result startScanExpress(bool fixedAngle, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result startScan(bool force, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT); virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count); protected: - u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout); + u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout = DEFAULT_TIMEOUT); u_result _waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); u_result _cacheScanData(); + void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount); + u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); + u_result _cacheCapsuledScanData(); u_result _sendCommand(_u8 cmd, const void * payload = NULL, size_t payloadsize = 0); - u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout); + u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout = DEFAULT_TIMEOUT); + u_result _waitSampleRate(rplidar_response_sample_rate_t * res, _u32 timeout = DEFAULT_TIMEOUT); void _disableDataGrabbing(); bool _isConnected; bool _isScanning; + bool _isSupportingMotorCtrl; rp::hal::Locker _lock; rp::hal::Event _dataEvt; rp::hal::serial_rxtx * _rxtx; rplidar_response_measurement_node_t _cached_scan_node_buf[2048]; size_t _cached_scan_node_count; - rp::hal::Thread _cachethread; - + _u16 _cached_sampleduration_std; + _u16 _cached_sampleduration_express; + + rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + bool _is_previous_capsuledataRdy; + + rp::hal::Thread _cachethread; }; diff --git a/sdk/src/sdkcommon.h b/sdk/src/sdkcommon.h index 932d627..aaf66f4 100644 --- a/sdk/src/sdkcommon.h +++ b/sdk/src/sdkcommon.h @@ -1,7 +1,13 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR SDK * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -25,17 +31,11 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * SDK Internal Common Header - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ #if defined(_WIN32) #include "arch\win32\arch_win32.h" +#elif defined(_MACOS) +#include "arch/macOS/arch_macOS.h" #elif defined(__GNUC__) #include "arch/linux/arch_linux.h" #else diff --git a/src/node.cpp b/src/node.cpp index 4059bcf..0a22049 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -1,45 +1,41 @@ /* - * Copyright (c) 2014, RoboPeak - * All rights reserved. + * RPLIDAR ROS NODE * - * Redistribution and use in source and binary forms, with or without + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, + * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ -/* - * RoboPeak LIDAR System - * RPlidar ROS Node - * - * Copyright 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * - */ - - #include "ros/ros.h" #include "sensor_msgs/LaserScan.h" - -#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header +#include "std_srvs/Empty.h" +#include "rplidar.h" #ifndef _countof #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) @@ -49,11 +45,13 @@ using namespace rp::standalone::rplidar; -void publish_scan(ros::Publisher *pub, - rplidar_response_measurement_node_t *nodes, +RPlidarDriver * drv = NULL; + +void publish_scan(ros::Publisher *pub, + rplidar_response_measurement_node_t *nodes, size_t node_count, ros::Time start, - double scan_time, bool inverted, - float angle_min, float angle_max, + double scan_time, bool inverted, + float angle_min, float angle_max, std::string frame_id) { static int scan_count = 0; @@ -63,36 +61,76 @@ void publish_scan(ros::Publisher *pub, 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 = + bool reversed = (angle_max > angle_min); + if ( reversed ) { + scan_msg.angle_min = M_PI - angle_max; + scan_msg.angle_max = M_PI - angle_min; + } else { + scan_msg.angle_min = M_PI - angle_min; + scan_msg.angle_max = M_PI - 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.range_max = 8.0; + scan_msg.intensities.resize(node_count); scan_msg.ranges.resize(node_count); - if (!inverted) { // assumes scan window at the top + bool reverse_data = (!inverted && reversed) || (inverted && !reversed); + if (!reverse_data) { for (size_t i = 0; i < node_count; i++) { - scan_msg.ranges[i] - = (float)nodes[i].distance_q2/4.0f/1000; + float read_value = (float) nodes[i].distance_q2/4.0f/1000; + if (read_value == 0.0) + scan_msg.ranges[i] = std::numeric_limits::infinity(); + else + scan_msg.ranges[i] = read_value; + scan_msg.intensities[i] = (float) (nodes[i].sync_quality >> 2); } } 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; + float read_value = (float)nodes[i].distance_q2/4.0f/1000; + if (read_value == 0.0) + scan_msg.ranges[node_count-1-i] = std::numeric_limits::infinity(); + else + scan_msg.ranges[node_count-1-i] = read_value; + scan_msg.intensities[node_count-1-i] = (float) (nodes[i].sync_quality >> 2); } } - scan_msg.intensities.resize(node_count); - for (size_t i = 0; i < node_count; i++) { - scan_msg.intensities[i] = (float)0; + pub->publish(scan_msg); +} + +bool getRPLIDARDeviceInfo(RPlidarDriver * drv) +{ + u_result op_result; + rplidar_response_device_info_t devinfo; + + op_result = drv->getDeviceInfo(devinfo); + if (IS_FAIL(op_result)) { + if (op_result == RESULT_OPERATION_TIMEOUT) { + fprintf(stderr, "Error, operation time out.\n"); + } else { + fprintf(stderr, "Error, unexpected error, code: %x\n", op_result); + } + return false; } - pub->publish(scan_msg); + // print out the device serial number, firmware and hardware version number.. + printf("RPLIDAR S/N: "); + for (int pos = 0; pos < 16 ;++pos) { + printf("%02X", devinfo.serialnum[pos]); + } + + printf("\n" + "Firmware Ver: %d.%02d\n" + "Hardware Rev: %d\n" + , devinfo.firmware_version>>8 + , devinfo.firmware_version & 0xFF + , (int)devinfo.hardware_version); + return true; } bool checkRPLIDARHealth(RPlidarDriver * drv) @@ -119,14 +157,37 @@ bool checkRPLIDARHealth(RPlidarDriver * drv) } } +bool stop_motor(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + if(!drv) + return false; + + ROS_DEBUG("Stop motor"); + drv->stop(); + drv->stopMotor(); + return true; +} + +bool start_motor(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + if(!drv) + return false; + ROS_DEBUG("Start motor"); + drv->startMotor(); + drv->startScan();; + return true; +} + int main(int argc, char * argv[]) { ros::init(argc, argv, "rplidar_node"); std::string serial_port; - int serial_baudrate; + int serial_baudrate = 115200; std::string frame_id; - bool inverted; - bool angle_compensate; + bool inverted = false; + bool angle_compensate = true; ros::NodeHandle nh; ros::Publisher scan_pub = nh.advertise("scan", 1000); @@ -134,14 +195,16 @@ int main(int argc, char * argv[]) { nh_private.param("serial_port", serial_port, "/dev/ttyUSB0"); nh_private.param("serial_baudrate", serial_baudrate, 115200); nh_private.param("frame_id", frame_id, "laser_frame"); - nh_private.param("inverted", inverted, "false"); - nh_private.param("angle_compensate", angle_compensate, "true"); + nh_private.param("inverted", inverted, false); + nh_private.param("angle_compensate", angle_compensate, true); + + printf("RPLIDAR running on ROS package rplidar_ros\n" + "SDK Version: "RPLIDAR_SDK_VERSION"\n"); u_result op_result; // create the driver instance - RPlidarDriver * drv = - RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT); + drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT); if (!drv) { fprintf(stderr, "Create Driver fail, exit\n"); @@ -156,13 +219,21 @@ int main(int argc, char * argv[]) { return -1; } + // get rplidar device info + if (!getRPLIDARDeviceInfo(drv)) { + return -1; + } + // check health... if (!checkRPLIDARHealth(drv)) { RPlidarDriver::DisposeDriver(drv); return -1; } - // start scan... + ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor); + ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor); + + drv->startMotor(); drv->startScan(); ros::Time start_scan_time; @@ -203,8 +274,8 @@ int main(int argc, char * argv[]) { } publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count, - start_scan_time, scan_duration, inverted, - angle_min, angle_max, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, frame_id); } else { int start_node = 0, end_node = 0; @@ -219,9 +290,9 @@ int main(int argc, char * argv[]) { angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_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, + 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 == RESULT_OPERATION_FAIL) { @@ -229,9 +300,9 @@ int main(int argc, char * argv[]) { 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, + publish_scan(&scan_pub, nodes, count, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, frame_id); } } @@ -240,6 +311,8 @@ int main(int argc, char * argv[]) { } // done! + drv->stop(); + drv->stopMotor(); RPlidarDriver::DisposeDriver(drv); return 0; }