├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include ├── rplidar_node.hpp └── visibility.h ├── launch ├── rplidar.launch.py ├── rplidar_a3.launch.py ├── rplidar_s1.launch.py ├── rplidar_s1_tcp.launch.py ├── test_rplidar_a3.launch.py ├── view_rplidar.launch.py ├── view_rplidar_a3.launch.py ├── view_rplidar_s1.launch.py └── view_rplidar_s1_tcp.launch.py ├── package.xml ├── rplidar_A1.png ├── rplidar_A2.png ├── rviz └── rplidar.rviz ├── scripts ├── create_udev_rules.sh ├── delete_udev_rules.sh └── rplidar.rules ├── sdk ├── README.txt ├── include │ ├── rplidar.h │ ├── rplidar_cmd.h │ ├── rplidar_driver.h │ ├── rplidar_protocol.h │ └── rptypes.h └── src │ ├── arch │ ├── linux │ │ ├── arch_linux.h │ │ ├── net_serial.cpp │ │ ├── net_serial.h │ │ ├── net_socket.cpp │ │ ├── thread.hpp │ │ ├── timer.cpp │ │ └── timer.h │ ├── macOS │ │ ├── arch_macOS.h │ │ ├── net_serial.cpp │ │ ├── net_serial.h │ │ ├── net_socket.cpp │ │ ├── thread.hpp │ │ ├── timer.cpp │ │ └── timer.h │ └── win32 │ │ ├── arch_win32.h │ │ ├── net_serial.cpp │ │ ├── net_serial.h │ │ ├── net_socket.cpp │ │ ├── timer.cpp │ │ ├── timer.h │ │ └── winthread.hpp │ ├── hal │ ├── abs_rxtx.h │ ├── assert.h │ ├── byteops.h │ ├── event.h │ ├── locker.h │ ├── socket.h │ ├── thread.cpp │ ├── thread.h │ ├── types.h │ └── util.h │ ├── rplidar_driver.cpp │ ├── rplidar_driver_TCP.h │ ├── rplidar_driver_impl.h │ ├── rplidar_driver_serial.h │ └── sdkcommon.h └── src ├── rplidar_node.cpp └── standalone_rplidar.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | launch/__pycache__/ 2 | *.swp 3 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rplidar_ros 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.0 (2019-02-22) 6 | ------------------ 7 | * Update RPLIDAR SDK to 1.10.0 8 | * [new feature] support Rplidar S1 9 | * Contributors: tony,WubinXia 10 | 11 | 1.9.0 (2018-08-24) 12 | ------------------ 13 | * Update RPLIDAR SDK to 1.9.0 14 | * [new feature] support baudrate 57600 and 1382400, support HQ scan response 15 | * [bugfix] TCP channel doesn't work 16 | * [improvement] Print warning messages when deprecated APIs are called; imporve angular accuracy for ultra capsuled scan points 17 | * Contributors: tony,kint 18 | 19 | 2.0.2 (2021-05-27) 20 | ------------------ 21 | * Remove test_rplidar.launch.py, since relevant executables no longer exist (`#24 `_) 22 | * Contributors: Hunter L. Allen 23 | 24 | 2.0.1 (2020-09-13) 25 | ------------------ 26 | * Remove old driver (`#21 `_) 27 | * Remove old rplidar driver in favor of the component version 28 | * Lint the source 29 | * Fix incompatibilities with slam_toolbox (`#20 `_) 30 | * Fix incompatibilities with slam_toolbox: 31 | - Fix angle compensate mode to publish angle compensated values 32 | - Fix angle_increment calculation 33 | - Add optional flip_x_axis option to deal with issue discussed here: https://github.com/SteveMacenski/slam_toolbox/issues/198. Flip x-axis can be used when laser is mounted with motor behind it as rotated TF laser frame doesn't seem to work with slam_toolbox. 34 | * Fix whitespace 35 | * Fix node count for component implementation (`#19 `_) 36 | * Slam Toolbox compatibility (`#18 `_) 37 | (cherry picked from commit f21079fea8eca8946b5b4ae72f50b8d9f1ac46a2) 38 | * Fix building with GCC 10 (`#17 `_) 39 | * Contributors: Christen Lofland, Hunter L. Allen, justinIRBT 40 | 41 | 2.0.0 (2020-07-15) 42 | ------------------ 43 | * Update SDK to Version 0.12.0 (`#14 `_) 44 | * Register the rclcpp component 45 | * Update RPLIDAR SDK to version 1.12.0 46 | * Update ROS 2 parameters and use node's clock instance (`#9 `_) 47 | * Update ROS 2 parameters and use node's clock instance 48 | * Fix scan_mode listing output 49 | * Stop motors and exit when set_scan_mode() call fails 50 | * Fix compilation with eloquent (`#6 `_) 51 | * Use Composition node with launch files (`#4 `_) 52 | * Composable nodes (`#3 `_) 53 | * Begin implementation of composable rplidar_ros::rplidar_node 54 | * Declare composition node library in CMake, as well as continue the port 55 | * Get to a compiling state 56 | * Add start/stop motor callbacks + more driver setup 57 | * Add publish loop for scans 58 | * Add composition node 59 | * Lint 60 | * Port rviz and launch files to ROS2 (`#2 `_) 61 | * Port non-rviz launch files to ROS2 62 | * Compatibility with rviz2 63 | * revert whitespace changes 64 | * Port the remaining launch files to ROS2 65 | * Revert more whitespace changes 66 | * Fix luanch and rviz install path indent level 67 | * Ros2 port (`#1 `_) 68 | ROS 2 port 69 | * Port CMakeLists.txt 70 | * Port package.xml 71 | * Port client.cpp 72 | * Port node.cpp 73 | Fix compilation 74 | * Support TCP 75 | * upgrade sdk 1.10.0 76 | * upgrade sdk 1.9.0 77 | [new feature] support baudrate 57600 and 1382400, support HQ scan response 78 | [bugfix] TCP channel doesn't work 79 | [improvement] Print warning messages when deprecated APIs are called; imporve angular accuracy for ultra capsuled scan points 80 | * [bugfix]modify scan_mode at test_rplidar.launch and test_rplidar_a3.launch 81 | * Contributors: Dan Rose, Hunter L. Allen, WubinXia, kint 82 | 83 | 1.7.0 (2018-07-19) 84 | ------------------ 85 | * Update RPLIDAR SDK to 1.7.0 86 | * support scan points farther than 16.38m 87 | * upport display and set scan mode 88 | * Contributors: kint 89 | 90 | 91 | 1.6.0 (2018-05-21) 92 | ------------------ 93 | * Release 1.6.0. 94 | * Update RPLIDAR SDK to 1.6.0 95 | * Support new product RPLIDAR A3(default 16K model and max_distance 25m) 96 | * Contributors: kint 97 | 98 | 99 | 1.5.7 (2016-12-15) 100 | ------------------ 101 | * Release 1.5.7. 102 | * Update RPLIDAR SDK to 1.5.7 103 | * Fixed the motor default speed at 10 HZ. Extend the measurement of max_distance from 6m to 8m. 104 | * Contributors: kint 105 | 106 | 1.5.5 (2016-08-23) 107 | ------------------ 108 | * Release 1.5.5. 109 | * Update RPLIDAR SDK to 1.5.5 110 | * Add RPLIDAR information print, and fix the standard motor speed of RPLIDAR A2. 111 | * Contributors: kint 112 | 113 | 1.5.4 (2016-06-02) 114 | ------------------ 115 | * Release 1.5.4. 116 | * Update RPLIDAR SDK to 1.5.4 117 | * Support RPLIDAR A2 118 | * Contributors: kint 119 | 120 | 1.5.2 (2016-04-29) 121 | ------------------ 122 | * Release 1.5.2. 123 | * Update RPLIDAR SDK to 1.5.2 124 | * Support RPLIDAR A2 125 | * Contributors: kint 126 | 127 | 1.0.1 (2014-06-03) 128 | ------------------ 129 | * Release 1.0.1. 130 | * Add angle compensate mechanism to compatible with ROS scan message 131 | * Add RPLIDAR sdk to the repo. 132 | * First release of RPLIDAR ROS package (1.0.0) 133 | * Initial commit 134 | * Contributors: Ling, RoboPeak Public Repos 135 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.9.5) 2 | project(rplidar_ros) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -pedantic) 10 | endif() 11 | 12 | set(RPLIDAR_SDK_PATH "./sdk/") 13 | 14 | FILE(GLOB RPLIDAR_SDK_SRC 15 | "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp" 16 | "${RPLIDAR_SDK_PATH}/src/hal/*.cpp" 17 | "${RPLIDAR_SDK_PATH}/src/*.cpp" 18 | ) 19 | 20 | set(req_deps 21 | "rclcpp" 22 | "sensor_msgs" 23 | "std_srvs" 24 | "rclcpp_components" 25 | ) 26 | 27 | find_package(ament_cmake_auto REQUIRED) 28 | find_package(ament_cmake_ros REQUIRED) 29 | 30 | ament_auto_find_build_dependencies(REQUIRED ${req_deps}) 31 | 32 | include_directories( 33 | ${RPLIDAR_SDK_PATH}/include 34 | ${RPLIDAR_SDK_PATH}/src 35 | include 36 | ) 37 | 38 | # build composition node 39 | add_library(rplidar_composition_node SHARED 40 | src/rplidar_node.cpp 41 | ${RPLIDAR_SDK_SRC} 42 | ) 43 | ament_target_dependencies(rplidar_composition_node ${req_deps}) 44 | rclcpp_components_register_nodes(rplidar_composition_node "rplidar_ros::rplidar_node") 45 | 46 | # build composition node 47 | ament_auto_add_executable(rplidar_composition src/standalone_rplidar.cpp) 48 | target_link_libraries(rplidar_composition rplidar_composition_node) 49 | ament_target_dependencies(rplidar_composition ${req_deps}) 50 | 51 | install( 52 | TARGETS rplidar_composition 53 | DESTINATION lib/${PROJECT_NAME} 54 | ) 55 | 56 | install( 57 | DIRECTORY launch rviz 58 | DESTINATION share/${PROJECT_NAME} 59 | ) 60 | 61 | # Install the shared library composition node 62 | install( 63 | TARGETS rplidar_composition_node 64 | ARCHIVE DESTINATION lib 65 | LIBRARY DESTINATION lib 66 | RUNTIME DESTINATION bin 67 | ) 68 | 69 | # TODO(hallen): port this 70 | # install(DIRECTORY sdk 71 | # USE_SOURCE_PERMISSIONS 72 | # ) 73 | 74 | ament_auto_package() 75 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009 - 2014 RoboPeak Team 2 | Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | NOTE: I just made some small change to make it compatible with ROS 2 Humble 2 | Hawksbill, Ubuntu 22.04 and Raspberry Pi 4. tested only with Rplidar A1 3 | 4 | RPLIDAR ROS package 5 | ===================================================================== 6 | 7 | ROS node and test application for RPLIDAR 8 | 9 | Visit following Website for more details about RPLIDAR: 10 | 11 | rplidar roswiki: http://wiki.ros.org/rplidar 12 | 13 | rplidar HomePage: http://www.slamtec.com/en/Lidar 14 | 15 | rplidar SDK: https://github.com/Slamtec/rplidar_sdk 16 | 17 | rplidar Tutorial: https://github.com/robopeak/rplidar_ros/wiki 18 | 19 | How to build rplidar ros package 20 | ===================================================================== 21 | 22 | 1) Clone this project to your colcon workspace src folder 23 | 2) Install Eloquent ROS2 and colcon compiler. 24 | 25 | 26 | ``` 27 | cd [your-ros-package-directory]/src 28 | 29 | git clone git@github.com:babakhani/rplidar_ros2.git 30 | 31 | cd [your-ros-package-directory] 32 | 33 | colcon build --symlink-install 34 | 35 | source ./install/setup.bash 36 | ``` 37 | 38 | Check if package exist 39 | 40 | ``` 41 | ros2 pkg list | grep rplidar 42 | ``` 43 | 44 | I. Run rplidar node and view in the rviz 45 | ------------------------------------------------------------ 46 | 47 | ``` 48 | ros2 launch rplidar_ros view_rplidar.launch.py 49 | ``` 50 | -------------------------------------------------------------------------------- /include/rplidar_node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR ROS NODE 3 | * 4 | * Copyright (c) 2019 Hunter L. Allen 5 | */ 6 | /* 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 19 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 20 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 21 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 22 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 23 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 24 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 25 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 26 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 27 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | */ 30 | #ifndef RPLIDAR_NODE_HPP_ 31 | #define RPLIDAR_NODE_HPP_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | #ifndef _countof 47 | #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) 48 | #endif 49 | 50 | #define M_PI 3.1415926535897932384626433832795 51 | 52 | namespace 53 | { 54 | using LaserScan = sensor_msgs::msg::LaserScan; 55 | using LaserScanPub = rclcpp::Publisher::SharedPtr; 56 | using StartMotorService = rclcpp::Service::SharedPtr; 57 | using StopMotorService = rclcpp::Service::SharedPtr; 58 | using RPlidarDriver = rp::standalone::rplidar::RPlidarDriver; 59 | using RplidarScanMode = rp::standalone::rplidar::RplidarScanMode; 60 | using Clock = rclcpp::Clock::SharedPtr; 61 | using ResponseNodeArray = std::unique_ptr; 62 | using EmptyRequest = std::shared_ptr; 63 | using EmptyResponse = std::shared_ptr; 64 | using Timer = rclcpp::TimerBase::SharedPtr; 65 | using namespace std::chrono_literals; 66 | } 67 | 68 | namespace rplidar_ros 69 | { 70 | 71 | constexpr double deg_2_rad(double x) 72 | { 73 | return x * M_PI / 180.0; 74 | } 75 | 76 | static float getAngle(const rplidar_response_measurement_node_hq_t & node) 77 | { 78 | return node.angle_z_q14 * 90.f / 16384.f; 79 | } 80 | 81 | class RPLIDAR_ROS_PUBLIC rplidar_node : public rclcpp::Node 82 | { 83 | public: 84 | explicit rplidar_node(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 85 | virtual ~rplidar_node(); 86 | 87 | void publish_scan(const double scan_time, ResponseNodeArray nodes, size_t node_count); 88 | 89 | /* service callbacks */ 90 | void stop_motor(const EmptyRequest req, EmptyResponse res); 91 | void start_motor(const EmptyRequest req, EmptyResponse res); 92 | 93 | private: 94 | bool getRPLIDARDeviceInfo() const; 95 | bool checkRPLIDARHealth() const; 96 | bool set_scan_mode(); 97 | void publish_loop(); 98 | 99 | /* parameters */ 100 | std::string channel_type_; 101 | std::string tcp_ip_; 102 | std::string serial_port_; 103 | std::string topic_name_; 104 | int tcp_port_; 105 | int serial_baudrate_; 106 | std::string frame_id_; 107 | bool inverted_; 108 | bool angle_compensate_; 109 | bool flip_x_axis_; 110 | int m_angle_compensate_multiple; 111 | std::string scan_mode_; 112 | /* Publisher */ 113 | LaserScanPub m_publisher; 114 | /* Services */ 115 | StopMotorService m_stop_motor_service; 116 | StartMotorService m_start_motor_service; 117 | /* SDK Pointer */ 118 | RPlidarDriver * m_drv = nullptr; 119 | /* Timer */ 120 | Timer m_timer; 121 | /* Scan Times */ 122 | size_t m_scan_count = 0; 123 | double max_distance = 8.0f; 124 | double angle_min = deg_2_rad(0); 125 | double angle_max = deg_2_rad(359); 126 | const float min_distance = 0.15f; 127 | }; 128 | 129 | } // namespace rplidar_ros 130 | 131 | #endif // RPLIDAR_NODE_HPP_ 132 | -------------------------------------------------------------------------------- /include/visibility.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef VISIBILITY_H_ 16 | #define VISIBILITY_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | 28 | #ifdef __GNUC__ 29 | #define RPLIDAR_ROS_EXPORT __attribute__ ((dllexport)) 30 | #define RPLIDAR_ROS_IMPORT __attribute__ ((dllimport)) 31 | #else 32 | #define RPLIDAR_ROS_EXPORT __declspec(dllexport) 33 | #define RPLIDAR_ROS_IMPORT __declspec(dllimport) 34 | #endif 35 | 36 | #ifdef RPLIDAR_ROS_DLL 37 | #define RPLIDAR_ROS_PUBLIC RPLIDAR_ROS_EXPORT 38 | #else 39 | #define RPLIDAR_ROS_PUBLIC RPLIDAR_ROS_IMPORT 40 | #endif 41 | 42 | #define RPLIDAR_ROS_PUBLIC_TYPE RPLIDAR_ROS_PUBLIC 43 | 44 | #define RPLIDAR_ROS_LOCAL 45 | 46 | #else 47 | 48 | #define RPLIDAR_ROS_EXPORT __attribute__ ((visibility("default"))) 49 | #define RPLIDAR_ROS_IMPORT 50 | 51 | #if __GNUC__ >= 4 52 | #define RPLIDAR_ROS_PUBLIC __attribute__ ((visibility("default"))) 53 | #define RPLIDAR_ROS_LOCAL __attribute__ ((visibility("hidden"))) 54 | #else 55 | #define RPLIDAR_ROS_PUBLIC 56 | #define RPLIDAR_ROS_LOCAL 57 | #endif 58 | 59 | #define RPLIDAR_ROS_PUBLIC_TYPE 60 | #endif 61 | 62 | #ifdef __cplusplus 63 | } 64 | #endif 65 | 66 | #endif // VISIBILITY_H_ 67 | -------------------------------------------------------------------------------- /launch/rplidar.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description(): 6 | return LaunchDescription([ 7 | Node( 8 | name='rplidar_composition', 9 | package='rplidar_ros', 10 | executable='rplidar_composition', 11 | output='screen', 12 | parameters=[{ 13 | 'serial_port': '/dev/ttyUSB0', 14 | 'serial_baudrate': 115200, # A1 / A2 15 | # 'serial_baudrate': 256000, # A3 16 | 'frame_id': 'laser', 17 | 'inverted': False, 18 | 'angle_compensate': True, 19 | }], 20 | ), 21 | ]) 22 | -------------------------------------------------------------------------------- /launch/rplidar_a3.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description(): 6 | return LaunchDescription([ 7 | Node( 8 | node_name='rplidar_composition', 9 | package='rplidar_ros', 10 | node_executable='rplidar_composition', 11 | output='screen', 12 | parameters=[{ 13 | 'serial_port': '/dev/ttyUSB0', 14 | 'serial_baudrate': 256000, # A3 15 | 'frame_id': 'laser', 16 | 'inverted': False, 17 | 'angle_compensate': True, 18 | 'scan_mode': 'Sensitivity', 19 | }], 20 | ), 21 | ]) 22 | -------------------------------------------------------------------------------- /launch/rplidar_s1.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description(): 6 | return LaunchDescription([ 7 | Node( 8 | node_name='rplidar_composition', 9 | package='rplidar_ros', 10 | node_executable='rplidar_composition', 11 | output='screen', 12 | parameters=[{ 13 | 'serial_port': '/dev/ttyUSB0', 14 | 'serial_baudrate': 256000, 15 | 'frame_id': 'laser', 16 | 'inverted': False, 17 | 'angle_compensate': True, 18 | }], 19 | ), 20 | ]) 21 | -------------------------------------------------------------------------------- /launch/rplidar_s1_tcp.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description(): 6 | return LaunchDescription([ 7 | Node( 8 | node_name='rplidar_composition', 9 | package='rplidar_ros', 10 | node_executable='rplidar_composition', 11 | output='screen', 12 | parameters=[{ 13 | 'channel_type': 'tcp', 14 | 'tcp_ip': '192.168.0.7', 15 | 'tcp_port': 20108, 16 | 'frame_id': 'laser', 17 | 'inverted': False, 18 | 'angle_compensate': True, 19 | }] 20 | ), 21 | ]) 22 | -------------------------------------------------------------------------------- /launch/test_rplidar_a3.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description(): 6 | return LaunchDescription([ 7 | Node( 8 | node_name='rplidar_composition', 9 | package='rplidar_ros', 10 | node_executable='rplidar_composition', 11 | output='screen', 12 | parameters=[{ 13 | 'serial_port': '/dev/ttyUSB0', 14 | 'serial_baudrate': 115200, 15 | 'frame_id': 'laser', 16 | 'inverted': False, 17 | 'angle_compensate': True, 18 | 'scan_mode': 'Sensitivity', 19 | }], 20 | ), 21 | Node( 22 | node_name='rplidarNodeClient', 23 | package='rplidar_ros', 24 | node_executable='rplidarNodeClient', 25 | output='screen', 26 | ), 27 | ]) 28 | -------------------------------------------------------------------------------- /launch/view_rplidar.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import IncludeLaunchDescription 3 | from launch.launch_description_sources import PythonLaunchDescriptionSource 4 | from launch.substitutions import ThisLaunchFileDir 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | return LaunchDescription([ 10 | IncludeLaunchDescription(PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rplidar.launch.py'])), 11 | Node( 12 | package='rviz2', 13 | executable='rviz2', 14 | output='screen', 15 | arguments=['-d', [ThisLaunchFileDir(), '/../rviz/rplidar.rviz']], 16 | ) 17 | ]) 18 | -------------------------------------------------------------------------------- /launch/view_rplidar_a3.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import IncludeLaunchDescription 3 | from launch.launch_description_sources import PythonLaunchDescriptionSource 4 | from launch.substitutions import ThisLaunchFileDir 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | return LaunchDescription([ 10 | IncludeLaunchDescription(PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rplidar_a3.launch.py'])), 11 | Node( 12 | package='rviz2', 13 | node_executable='rviz2', 14 | output='screen', 15 | arguments=['-d', [ThisLaunchFileDir(), '/../rviz/rplidar.rviz']], 16 | ) 17 | ]) 18 | -------------------------------------------------------------------------------- /launch/view_rplidar_s1.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import IncludeLaunchDescription 3 | from launch.launch_description_sources import PythonLaunchDescriptionSource 4 | from launch.substitutions import ThisLaunchFileDir 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | return LaunchDescription([ 10 | IncludeLaunchDescription(PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rplidar_s1.launch.py'])), 11 | Node( 12 | package='rviz2', 13 | node_executable='rviz2', 14 | output='screen', 15 | arguments=['-d', [ThisLaunchFileDir(), '/../rviz/rplidar.rviz']], 16 | ) 17 | ]) 18 | -------------------------------------------------------------------------------- /launch/view_rplidar_s1_tcp.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import IncludeLaunchDescription 3 | from launch.launch_description_sources import PythonLaunchDescriptionSource 4 | from launch.substitutions import ThisLaunchFileDir 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | return LaunchDescription([ 10 | IncludeLaunchDescription(PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rplidar_s1_tcp.launch.py'])), 11 | Node( 12 | package='rviz2', 13 | node_executable='rviz2', 14 | output='screen', 15 | arguments=['-d', [ThisLaunchFileDir(), '/../rviz/rplidar.rviz']], 16 | ) 17 | ]) 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rplidar_ros 4 | 2.0.2 5 | The rplidar ros package, support rplidar A2/A1 and A3/S1 6 | 7 | Hunter L. Allen 8 | Slamtec ROS Maintainer 9 | BSD 10 | 11 | ament_cmake_auto 12 | ament_cmake_ros 13 | 14 | rclcpp 15 | 16 | sensor_msgs 17 | std_srvs 18 | rclcpp_components 19 | 20 | rclcpp 21 | 22 | sensor_msgs 23 | std_srvs 24 | rclcpp_components 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /rplidar_A1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/babakhani/rplidar_ros2/20fbde2145a4fb5c720e11557ab3fe812725993f/rplidar_A1.png -------------------------------------------------------------------------------- /rplidar_A2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/babakhani/rplidar_ros2/20fbde2145a4fb5c720e11557ab3fe812725993f/rplidar_A2.png -------------------------------------------------------------------------------- /rviz/rplidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RPLidarLaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 397 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | Visualization Manager: 26 | Class: "" 27 | Displays: 28 | - Alpha: 0.5 29 | Cell Size: 1 30 | Class: rviz_default_plugins/Grid 31 | Color: 160; 160; 164 32 | Enabled: true 33 | Line Style: 34 | Line Width: 0.029999999329447746 35 | Value: Lines 36 | Name: Grid 37 | Normal Cell Count: 0 38 | Offset: 39 | X: 0 40 | Y: 0 41 | Z: 0 42 | Plane: XY 43 | Plane Cell Count: 10 44 | Reference Frame: 45 | Value: true 46 | - Alpha: 1 47 | Autocompute Intensity Bounds: true 48 | Autocompute Value Bounds: 49 | Max Value: 0 50 | Min Value: 0 51 | Value: true 52 | Axis: Z 53 | Channel Name: intensity 54 | Class: rviz_default_plugins/LaserScan 55 | Color: 255; 255; 255 56 | Color Transformer: AxisColor 57 | Decay Time: 0 58 | Enabled: true 59 | Invert Rainbow: false 60 | Max Color: 255; 255; 255 61 | Max Intensity: 4096 62 | Min Color: 0; 0; 0 63 | Min Intensity: 0 64 | Name: RPLidarLaserScan 65 | Position Transformer: XYZ 66 | Queue Size: 1000 67 | Selectable: true 68 | Size (Pixels): 5 69 | Size (m): 0.029999999329447746 70 | Style: Squares 71 | Topic: /scan 72 | Unreliable: false 73 | Use Fixed Frame: true 74 | Use rainbow: true 75 | Value: true 76 | Enabled: true 77 | Global Options: 78 | Background Color: 48; 48; 48 79 | Fixed Frame: laser 80 | Frame Rate: 30 81 | Name: root 82 | Tools: 83 | - Class: rviz_default_plugins/MoveCamera 84 | - Class: rviz_default_plugins/Select 85 | - Class: rviz_default_plugins/FocusCamera 86 | - Class: rviz_default_plugins/Measure 87 | Line color: 128; 128; 0 88 | - Class: rviz_default_plugins/SetInitialPose 89 | Topic: /initialpose 90 | - Class: rviz_default_plugins/SetGoal 91 | Topic: /move_base_simple/goal 92 | - Class: rviz_default_plugins/PublishPoint 93 | Single click: true 94 | Topic: /clicked_point 95 | Transformation: 96 | Current: 97 | Class: rviz_default_plugins/TF 98 | Value: true 99 | Views: 100 | Current: 101 | Class: rviz_default_plugins/Orbit 102 | Distance: 10 103 | Enable Stereo Rendering: 104 | Stereo Eye Separation: 0.05999999865889549 105 | Stereo Focal Distance: 1 106 | Swap Stereo Eyes: false 107 | Value: false 108 | Focal Point: 109 | X: 0 110 | Y: 0 111 | Z: 0 112 | Focal Shape Fixed Size: false 113 | Focal Shape Size: 0.05000000074505806 114 | Invert Z Axis: false 115 | Name: Current View 116 | Near Clip Distance: 0.009999999776482582 117 | Pitch: 0.7853981852531433 118 | Target Frame: 119 | Value: Orbit (rviz) 120 | Yaw: 0.7853981852531433 121 | Saved: ~ 122 | Window Geometry: 123 | Displays: 124 | collapsed: false 125 | Height: 626 126 | Hide Left Dock: false 127 | Hide Right Dock: false 128 | QMainWindow State: 000000ff00000000fd00000004000000000000017500000218fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000218000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000218fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000218000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000022f0000021800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 129 | Selection: 130 | collapsed: false 131 | Tool Properties: 132 | collapsed: false 133 | Views: 134 | collapsed: false 135 | Width: 1215 136 | X: 67 137 | Y: 27 138 | -------------------------------------------------------------------------------- /scripts/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "remap the device serial port(ttyUSBX) to rplidar" 4 | echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB" 5 | echo "start copy rplidar.rules to /etc/udev/rules.d/" 6 | echo "`rospack find rplidar_ros`/scripts/rplidar.rules" 7 | sudo cp `rospack find rplidar_ros`/scripts/rplidar.rules /etc/udev/rules.d 8 | echo " " 9 | echo "Restarting udev" 10 | echo "" 11 | sudo service udev reload 12 | sudo service udev restart 13 | echo "finish " 14 | -------------------------------------------------------------------------------- /scripts/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "delete remap the device serial port(ttyUSBX) to rplidar" 4 | echo "sudo rm /etc/udev/rules.d/rplidar.rules" 5 | sudo rm /etc/udev/rules.d/rplidar.rules 6 | echo " " 7 | echo "Restarting udev" 8 | echo "" 9 | sudo service udev reload 10 | sudo service udev restart 11 | echo "finish delete" 12 | -------------------------------------------------------------------------------- /scripts/rplidar.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by rplidar 2 | # 3 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar" 4 | 5 | -------------------------------------------------------------------------------- /sdk/README.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009 - 2014 RoboPeak Team 2 | Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | 27 | 28 | This folder contains RPLIDAR SDK source code which is provided by RoboPeak. 29 | 30 | RoboPeak Website: http://www.robopeak.com 31 | SlamTec HomePage: http://www.slamtec.com 32 | RPLIDAR_SDK_VERSION: 1.12.0 33 | Note: The SDK version may not up-to-date. 34 | rplidar product: http://www.slamtec.com/en/Lidar 35 | -------------------------------------------------------------------------------- /sdk/include/rplidar.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include "hal/types.h" 39 | #include "rplidar_protocol.h" 40 | #include "rplidar_cmd.h" 41 | 42 | #include "rplidar_driver.h" 43 | 44 | #define RPLIDAR_SDK_VERSION "1.12.0" 45 | -------------------------------------------------------------------------------- /sdk/include/rplidar_cmd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "rplidar_protocol.h" 38 | 39 | // Commands 40 | //----------------------------------------- 41 | 42 | // Commands without payload and response 43 | #define RPLIDAR_CMD_STOP 0x25 44 | #define RPLIDAR_CMD_SCAN 0x20 45 | #define RPLIDAR_CMD_FORCE_SCAN 0x21 46 | #define RPLIDAR_CMD_RESET 0x40 47 | 48 | 49 | // Commands without payload but have response 50 | #define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 51 | #define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 52 | 53 | #define RPLIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17 54 | 55 | #define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8 56 | 57 | // Commands with payload and have response 58 | #define RPLIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 59 | #define RPLIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24 60 | #define RPLIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24 61 | #define RPLIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24 62 | //add for A2 to set RPLIDAR motor pwm when using accessory board 63 | #define RPLIDAR_CMD_SET_MOTOR_PWM 0xF0 64 | #define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF 65 | 66 | #if defined(_WIN32) 67 | #pragma pack(1) 68 | #endif 69 | 70 | 71 | // Payloads 72 | // ------------------------------------------ 73 | #define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL 0 74 | #define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail 75 | //for express working flag(extending express scan protocol) 76 | #define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001 77 | #define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002 78 | 79 | //for ultra express working flag 80 | #define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001 81 | #define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002 82 | 83 | #define RPLIDAR_HQ_SCAN_FLAG_CCW (0x1<<0) 84 | #define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER (0x1<<1) 85 | #define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE (0x1<<2) 86 | 87 | typedef struct _rplidar_payload_express_scan_t { 88 | _u8 working_mode; 89 | _u16 working_flags; 90 | _u16 param; 91 | } __attribute__((packed)) rplidar_payload_express_scan_t; 92 | 93 | typedef struct _rplidar_payload_hq_scan_t { 94 | _u8 flag; 95 | _u8 reserved[32]; 96 | } __attribute__((packed)) rplidar_payload_hq_scan_t; 97 | 98 | typedef struct _rplidar_payload_get_scan_conf_t { 99 | _u32 type; 100 | _u8 reserved[32]; 101 | } __attribute__((packed)) rplidar_payload_get_scan_conf_t; 102 | #define MAX_MOTOR_PWM 1023 103 | #define DEFAULT_MOTOR_PWM 660 104 | typedef struct _rplidar_payload_motor_pwm_t { 105 | _u16 pwm_value; 106 | } __attribute__((packed)) rplidar_payload_motor_pwm_t; 107 | 108 | typedef struct _rplidar_payload_acc_board_flag_t { 109 | _u32 reserved; 110 | } __attribute__((packed)) rplidar_payload_acc_board_flag_t; 111 | 112 | typedef struct _rplidar_payload_hq_spd_ctrl_t { 113 | _u16 rpm; 114 | } __attribute__((packed)) rplidar_payload_hq_spd_ctrl_t; 115 | 116 | // Response 117 | // ------------------------------------------ 118 | #define RPLIDAR_ANS_TYPE_DEVINFO 0x4 119 | #define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6 120 | 121 | #define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81 122 | // Added in FW ver 1.17 123 | #define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82 124 | #define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83 125 | 126 | 127 | // Added in FW ver 1.17 128 | #define RPLIDAR_ANS_TYPE_SAMPLE_RATE 0x15 129 | //added in FW ver 1.23alpha 130 | #define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84 131 | //added in FW ver 1.24 132 | #define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20 133 | #define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21 134 | #define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED 0x85 135 | #define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF 136 | 137 | #define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1) 138 | typedef struct _rplidar_response_acc_board_flag_t { 139 | _u32 support_flag; 140 | } __attribute__((packed)) rplidar_response_acc_board_flag_t; 141 | 142 | 143 | #define RPLIDAR_STATUS_OK 0x0 144 | #define RPLIDAR_STATUS_WARNING 0x1 145 | #define RPLIDAR_STATUS_ERROR 0x2 146 | 147 | #define RPLIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) 148 | #define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 149 | 150 | #define RPLIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0) 151 | 152 | #define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) 153 | #define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 154 | 155 | typedef struct _rplidar_response_sample_rate_t { 156 | _u16 std_sample_duration_us; 157 | _u16 express_sample_duration_us; 158 | } __attribute__((packed)) rplidar_response_sample_rate_t; 159 | 160 | typedef struct _rplidar_response_measurement_node_t { 161 | _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; 162 | _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; 163 | _u16 distance_q2; 164 | } __attribute__((packed)) rplidar_response_measurement_node_t; 165 | 166 | //[distance_sync flags] 167 | #define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3) 168 | #define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC) 169 | 170 | typedef struct _rplidar_response_cabin_nodes_t { 171 | _u16 distance_angle_1; // see [distance_sync flags] 172 | _u16 distance_angle_2; // see [distance_sync flags] 173 | _u8 offset_angles_q3; 174 | } __attribute__((packed)) rplidar_response_cabin_nodes_t; 175 | 176 | 177 | #define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA 178 | #define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5 179 | 180 | #define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5 181 | 182 | #define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15) 183 | 184 | typedef struct _rplidar_response_capsule_measurement_nodes_t { 185 | _u8 s_checksum_1; // see [s_checksum_1] 186 | _u8 s_checksum_2; // see [s_checksum_1] 187 | _u16 start_angle_sync_q6; 188 | rplidar_response_cabin_nodes_t cabins[16]; 189 | } __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t; 190 | 191 | typedef struct _rplidar_response_dense_cabin_nodes_t { 192 | _u16 distance; 193 | } __attribute__((packed)) rplidar_response_dense_cabin_nodes_t; 194 | 195 | typedef struct _rplidar_response_dense_capsule_measurement_nodes_t { 196 | _u8 s_checksum_1; // see [s_checksum_1] 197 | _u8 s_checksum_2; // see [s_checksum_1] 198 | _u16 start_angle_sync_q6; 199 | rplidar_response_dense_cabin_nodes_t cabins[40]; 200 | } __attribute__((packed)) rplidar_response_dense_capsule_measurement_nodes_t; 201 | 202 | // ext1 : x2 boost mode 203 | 204 | #define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12 205 | #define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10 206 | 207 | typedef struct _rplidar_response_ultra_cabin_nodes_t { 208 | // 31 0 209 | // | predict2 10bit | predict1 10bit | major 12bit | 210 | _u32 combined_x3; 211 | } __attribute__((packed)) rplidar_response_ultra_cabin_nodes_t; 212 | 213 | typedef struct _rplidar_response_ultra_capsule_measurement_nodes_t { 214 | _u8 s_checksum_1; // see [s_checksum_1] 215 | _u8 s_checksum_2; // see [s_checksum_1] 216 | _u16 start_angle_sync_q6; 217 | rplidar_response_ultra_cabin_nodes_t ultra_cabins[32]; 218 | } __attribute__((packed)) rplidar_response_ultra_capsule_measurement_nodes_t; 219 | 220 | typedef struct rplidar_response_measurement_node_hq_t { 221 | _u16 angle_z_q14; 222 | _u32 dist_mm_q2; 223 | _u8 quality; 224 | _u8 flag; 225 | } __attribute__((packed)) rplidar_response_measurement_node_hq_t; 226 | 227 | typedef struct _rplidar_response_hq_capsule_measurement_nodes_t{ 228 | _u8 sync_byte; 229 | _u64 time_stamp; 230 | rplidar_response_measurement_node_hq_t node_hq[16]; 231 | _u32 crc32; 232 | }__attribute__((packed)) rplidar_response_hq_capsule_measurement_nodes_t; 233 | 234 | 235 | # define RPLIDAR_CONF_SCAN_COMMAND_STD 0 236 | # define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS 1 237 | # define RPLIDAR_CONF_SCAN_COMMAND_HQ 2 238 | # define RPLIDAR_CONF_SCAN_COMMAND_BOOST 3 239 | # define RPLIDAR_CONF_SCAN_COMMAND_STABILITY 4 240 | # define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5 241 | 242 | #define RPLIDAR_CONF_ANGLE_RANGE 0x00000000 243 | #define RPLIDAR_CONF_DESIRED_ROT_FREQ 0x00000001 244 | #define RPLIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002 245 | #define RPLIDAR_CONF_MIN_ROT_FREQ 0x00000004 246 | #define RPLIDAR_CONF_MAX_ROT_FREQ 0x00000005 247 | #define RPLIDAR_CONF_MAX_DISTANCE 0x00000060 248 | 249 | #define RPLIDAR_CONF_SCAN_MODE_COUNT 0x00000070 250 | #define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071 251 | #define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074 252 | #define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075 253 | #define RPLIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C 254 | #define RPLIDAR_CONF_SCAN_MODE_NAME 0x0000007F 255 | #define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4 256 | #define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5 257 | 258 | typedef struct _rplidar_response_get_lidar_conf{ 259 | _u32 type; 260 | _u8 payload[0]; 261 | }__attribute__((packed)) rplidar_response_get_lidar_conf_t; 262 | 263 | typedef struct _rplidar_response_set_lidar_conf{ 264 | _u32 result; 265 | }__attribute__((packed)) rplidar_response_set_lidar_conf_t; 266 | 267 | 268 | typedef struct _rplidar_response_device_info_t { 269 | _u8 model; 270 | _u16 firmware_version; 271 | _u8 hardware_version; 272 | _u8 serialnum[16]; 273 | } __attribute__((packed)) rplidar_response_device_info_t; 274 | 275 | typedef struct _rplidar_response_device_health_t { 276 | _u8 status; 277 | _u16 error_code; 278 | } __attribute__((packed)) rplidar_response_device_health_t; 279 | 280 | // Definition of the variable bit scale encoding mechanism 281 | #define RPLIDAR_VARBITSCALE_X2_SRC_BIT 9 282 | #define RPLIDAR_VARBITSCALE_X4_SRC_BIT 11 283 | #define RPLIDAR_VARBITSCALE_X8_SRC_BIT 12 284 | #define RPLIDAR_VARBITSCALE_X16_SRC_BIT 14 285 | 286 | #define RPLIDAR_VARBITSCALE_X2_DEST_VAL 512 287 | #define RPLIDAR_VARBITSCALE_X4_DEST_VAL 1280 288 | #define RPLIDAR_VARBITSCALE_X8_DEST_VAL 1792 289 | #define RPLIDAR_VARBITSCALE_X16_DEST_VAL 3328 290 | 291 | #define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \ 292 | ( (((0x1<<(_BITS_)) - RPLIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \ 293 | ((RPLIDAR_VARBITSCALE_X16_DEST_VAL - RPLIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \ 294 | ((RPLIDAR_VARBITSCALE_X8_DEST_VAL - RPLIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \ 295 | ((RPLIDAR_VARBITSCALE_X4_DEST_VAL - RPLIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \ 296 | RPLIDAR_VARBITSCALE_X2_DEST_VAL - 1) 297 | 298 | 299 | #if defined(_WIN32) 300 | #pragma pack() 301 | #endif 302 | -------------------------------------------------------------------------------- /sdk/include/rplidar_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | 38 | #ifndef __cplusplus 39 | #error "The RPlidar SDK requires a C++ compiler to be built" 40 | #endif 41 | 42 | #ifndef DEPRECATED 43 | #ifdef __GNUC__ 44 | #define DEPRECATED(func) func __attribute__ ((deprecated)) 45 | #elif defined(_MSC_VER) 46 | #define DEPRECATED(func) __declspec(deprecated) func 47 | #else 48 | #pragma message("WARNING: You need to implement DEPRECATED for this compiler") 49 | #define DEPRECATED(func) func 50 | #endif 51 | #endif 52 | 53 | namespace rp { namespace standalone{ namespace rplidar { 54 | 55 | struct RplidarScanMode { 56 | _u16 id; 57 | float us_per_sample; // microseconds per sample 58 | float max_distance; // max distance 59 | _u8 ans_type; // the answer type of the scam mode, its value should be RPLIDAR_ANS_TYPE_MEASUREMENT* 60 | char scan_mode[64]; // name of scan mode, max 63 characters 61 | }; 62 | 63 | enum { 64 | DRIVER_TYPE_SERIALPORT = 0x0, 65 | DRIVER_TYPE_TCP = 0x1, 66 | }; 67 | 68 | class ChannelDevice 69 | { 70 | public: 71 | virtual bool bind(const char*, uint32_t ) = 0; 72 | virtual bool open() {return true;} 73 | virtual void close() = 0; 74 | virtual void flush() {return;} 75 | virtual bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0; 76 | virtual int senddata(const _u8 * data, size_t size) = 0; 77 | virtual int recvdata(unsigned char * data, size_t size) = 0; 78 | virtual void setDTR() {return;} 79 | virtual void clearDTR() {return;} 80 | virtual void ReleaseRxTx() {return;} 81 | }; 82 | 83 | class RPlidarDriver { 84 | public: 85 | enum { 86 | DEFAULT_TIMEOUT = 2000, //2000 ms 87 | }; 88 | 89 | enum { 90 | MAX_SCAN_NODES = 8192, 91 | }; 92 | 93 | enum { 94 | LEGACY_SAMPLE_DURATION = 476, 95 | }; 96 | 97 | public: 98 | /// Create an RPLIDAR Driver Instance 99 | /// This interface should be invoked first before any other operations 100 | /// 101 | /// \param drivertype the connection type used by the driver. 102 | static RPlidarDriver * CreateDriver(_u32 drivertype = DRIVER_TYPE_SERIALPORT); 103 | 104 | /// Dispose the RPLIDAR Driver Instance specified by the drv parameter 105 | /// Applications should invoke this interface when the driver instance is no longer used in order to free memory 106 | static void DisposeDriver(RPlidarDriver * drv); 107 | 108 | 109 | /// Open the specified serial port and connect to a target RPLIDAR device 110 | /// 111 | /// \param port_path the device path of the serial port 112 | /// e.g. on Windows, it may be com3 or \\.\com10 113 | /// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc 114 | /// 115 | /// \param baudrate the baudrate used 116 | /// For most RPLIDAR models, the baudrate should be set to 115200 117 | /// 118 | /// \param flag other flags 119 | /// Reserved for future use, always set to Zero 120 | virtual u_result connect(const char *, _u32, _u32 flag = 0) = 0; 121 | 122 | 123 | /// Disconnect with the RPLIDAR and close the serial port 124 | virtual void disconnect() = 0; 125 | 126 | /// Returns TRUE when the connection has been established 127 | virtual bool isConnected() = 0; 128 | 129 | /// Ask the RPLIDAR core system to reset it self 130 | /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode. 131 | /// 132 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 133 | virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0; 134 | 135 | virtual u_result clearNetSerialRxCache() = 0; 136 | // FW1.24 137 | /// Get all scan modes that supported by lidar 138 | virtual u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; 139 | 140 | /// Get typical scan mode of lidar 141 | virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; 142 | 143 | /// Start scan 144 | /// 145 | /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. 146 | /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) 147 | /// \param options Scan options (please use 0) 148 | /// \param outUsedScanMode The scan mode selected by lidar 149 | virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0; 150 | 151 | /// Start scan in specific mode 152 | /// 153 | /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. 154 | /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) 155 | /// \param options Scan options (please use 0) 156 | /// \param outUsedScanMode The scan mode selected by lidar 157 | virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT) = 0; 158 | 159 | /// Retrieve the health status of the RPLIDAR 160 | /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. 161 | /// 162 | /// \param health The health status info returned from the RPLIDAR 163 | /// 164 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 165 | virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0; 166 | 167 | /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. 168 | /// 169 | /// \param info The device information returned from the RPLIDAR 170 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 171 | virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0; 172 | 173 | /// Get the sample duration information of the RPLIDAR. 174 | /// DEPRECATED, please use RplidarScanMode::us_per_sample 175 | /// 176 | /// \param rateInfo The sample duration information returned from the RPLIDAR 177 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 178 | DEPRECATED(virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT)) = 0; 179 | 180 | /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 and A3 only. 181 | /// 182 | /// \param pwm The motor pwm value would like to set 183 | virtual u_result setMotorPWM(_u16 pwm) = 0; 184 | 185 | /// Set the RPLIDAR's motor rpm, currently valid for tof lidar only. 186 | /// 187 | /// \param rpm The motor rpm value would like to set 188 | virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT) = 0; 189 | 190 | /// Start RPLIDAR's motor when using accessory board 191 | virtual u_result startMotor() = 0; 192 | 193 | /// Stop RPLIDAR's motor when using accessory board 194 | virtual u_result stopMotor() = 0; 195 | 196 | /// Check whether the device support motor control. 197 | /// Note: this API will disable grab. 198 | /// 199 | /// \param support Return the result. 200 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication. 201 | virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0; 202 | 203 | /// Check if the device is Tof lidar. 204 | /// Note: this API is effective if and only if getDeviceInfo has been called. 205 | /// 206 | /// \param support Return the result. 207 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication. 208 | virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT) = 0; 209 | 210 | /// Calculate RPLIDAR's current scanning frequency from the given scan data 211 | /// DEPRECATED, please use getFrequency(RplidarScanMode, size_t) 212 | /// 213 | /// Please refer to the application note doc for details 214 | /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data 215 | /// 216 | /// \param inExpressMode Indicate whether the RPLIDAR is in express mode 217 | /// \param count The number of sample nodes inside the given buffer 218 | /// \param frequency The scanning frequency (in HZ) calcuated by the interface. 219 | /// \param is4kmode Return whether the RPLIDAR is working on 4k sample rate mode. 220 | DEPRECATED(virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)) = 0; 221 | 222 | /// Calculate RPLIDAR's current scanning frequency from the given scan data 223 | /// Please refer to the application note doc for details 224 | /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data 225 | /// 226 | /// \param scanMode Lidar's current scan mode 227 | /// \param count The number of sample nodes inside the given buffer 228 | virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) = 0; 229 | 230 | /// Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode) 231 | /// A background thread will be created by the RPLIDAR driver to fetch the scan data continuously. 232 | /// User Application can use the grabScanData() interface to retrieved the scan data cached previous by this background thread. 233 | /// 234 | /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. 235 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication. 236 | virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0; 237 | 238 | /// Check whether the device support express mode. 239 | /// DEPRECATED, please use getAllSupportedScanModes 240 | /// 241 | /// \param support Return the result. 242 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication. 243 | DEPRECATED(virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT)) = 0; 244 | 245 | /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated 246 | /// 247 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 248 | virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0; 249 | 250 | 251 | /// Wait and grab a complete 0-360 degree scan data previously received. 252 | /// NOTE: This method only support distance less than 16.38 meters, for longer distance, please use grabScanDataHq 253 | /// The grabbed scan data returned by this interface always has the following charactistics: 254 | /// 255 | /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 256 | /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan 257 | /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. 258 | /// 259 | /// \param nodebuffer Buffer provided by the caller application to store the scan data 260 | /// 261 | /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). 262 | /// Once the interface returns, this parameter will store the actual received data count. 263 | /// 264 | /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. 265 | /// 266 | /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 267 | /// 268 | /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. 269 | DEPRECATED(virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT)) = 0; 270 | 271 | /// Wait and grab a complete 0-360 degree scan data previously received. 272 | /// The grabbed scan data returned by this interface always has the following charactistics: 273 | /// 274 | /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 275 | /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan 276 | /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. 277 | /// 278 | /// \param nodebuffer Buffer provided by the caller application to store the scan data 279 | /// 280 | /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). 281 | /// Once the interface returns, this parameter will store the actual received data count. 282 | /// 283 | /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. 284 | /// 285 | /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 286 | /// 287 | /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. 288 | virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0; 289 | 290 | /// Ascending the scan data according to the angle value in the scan. 291 | /// 292 | /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData 293 | /// 294 | /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). 295 | /// Once the interface returns, this parameter will store the actual received data count. 296 | /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 297 | DEPRECATED(virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count)) = 0; 298 | 299 | /// Ascending the scan data according to the angle value in the scan. 300 | /// 301 | /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData 302 | /// 303 | /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). 304 | /// Once the interface returns, this parameter will store the actual received data count. 305 | /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 306 | virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) = 0; 307 | 308 | /// Return received scan points even if it's not complete scan 309 | /// 310 | /// \param nodebuffer Buffer provided by the caller application to store the scan data 311 | /// 312 | /// \param count Once the interface returns, this parameter will store the actual received data count. 313 | /// 314 | /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 315 | DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0; 316 | 317 | /// Return received scan points even if it's not complete scan. 318 | /// 319 | /// \param nodebuffer Buffer provided by the caller application to store the scan data. This buffer must be initialized by 320 | /// the caller. 321 | /// 322 | /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). 323 | /// Once the interface returns, this parameter will store the actual received data count. 324 | /// 325 | /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 326 | /// The interface will return RESULT_REMAINING_DATA to indicate that the given buffer is full, but that there remains data to be read. 327 | virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0; 328 | 329 | virtual ~RPlidarDriver() {} 330 | protected: 331 | RPlidarDriver(){} 332 | 333 | public: 334 | ChannelDevice* _chanDev; 335 | }; 336 | 337 | 338 | 339 | 340 | }}} 341 | -------------------------------------------------------------------------------- /sdk/include/rplidar_protocol.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | // RP-Lidar Input Packets 38 | 39 | #define RPLIDAR_CMD_SYNC_BYTE 0xA5 40 | #define RPLIDAR_CMDFLAG_HAS_PAYLOAD 0x80 41 | 42 | 43 | #define RPLIDAR_ANS_SYNC_BYTE1 0xA5 44 | #define RPLIDAR_ANS_SYNC_BYTE2 0x5A 45 | 46 | #define RPLIDAR_ANS_PKTFLAG_LOOP 0x1 47 | 48 | #define RPLIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF 49 | #define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) 50 | 51 | #if defined(_WIN32) 52 | #pragma pack(1) 53 | #endif 54 | 55 | typedef struct _rplidar_cmd_packet_t { 56 | _u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE 57 | _u8 cmd_flag; 58 | _u8 size; 59 | _u8 data[0]; 60 | } __attribute__((packed)) rplidar_cmd_packet_t; 61 | 62 | 63 | typedef struct _rplidar_ans_header_t { 64 | _u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1 65 | _u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2 66 | _u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; 67 | _u8 type; 68 | } __attribute__((packed)) rplidar_ans_header_t; 69 | 70 | #if defined(_WIN32) 71 | #pragma pack() 72 | #endif 73 | -------------------------------------------------------------------------------- /sdk/include/rptypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | 38 | #ifdef _WIN32 39 | 40 | //fake stdint.h for VC only 41 | 42 | typedef signed char int8_t; 43 | typedef unsigned char uint8_t; 44 | 45 | typedef __int16 int16_t; 46 | typedef unsigned __int16 uint16_t; 47 | 48 | typedef __int32 int32_t; 49 | typedef unsigned __int32 uint32_t; 50 | 51 | typedef __int64 int64_t; 52 | typedef unsigned __int64 uint64_t; 53 | 54 | #else 55 | 56 | #include 57 | 58 | #endif 59 | 60 | 61 | //based on stdint.h 62 | typedef int8_t _s8; 63 | typedef uint8_t _u8; 64 | 65 | typedef int16_t _s16; 66 | typedef uint16_t _u16; 67 | 68 | typedef int32_t _s32; 69 | typedef uint32_t _u32; 70 | 71 | typedef int64_t _s64; 72 | typedef uint64_t _u64; 73 | 74 | #define __small_endian 75 | 76 | #ifndef __GNUC__ 77 | #define __attribute__(x) 78 | #endif 79 | 80 | 81 | // The _word_size_t uses actual data bus width of the current CPU 82 | #ifdef _AVR_ 83 | typedef _u8 _word_size_t; 84 | #define THREAD_PROC 85 | #elif defined (WIN64) 86 | typedef _u64 _word_size_t; 87 | #define THREAD_PROC __stdcall 88 | #elif defined (WIN32) 89 | typedef _u32 _word_size_t; 90 | #define THREAD_PROC __stdcall 91 | #elif defined (__GNUC__) 92 | typedef unsigned long _word_size_t; 93 | #define THREAD_PROC 94 | #elif defined (__ICCARM__) 95 | typedef _u32 _word_size_t; 96 | #define THREAD_PROC 97 | #endif 98 | 99 | 100 | typedef uint32_t u_result; 101 | 102 | #define RESULT_OK 0 103 | #define RESULT_FAIL_BIT 0x80000000 104 | #define RESULT_ALREADY_DONE 0x20 105 | #define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) 106 | #define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) 107 | #define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) 108 | #define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) 109 | #define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) 110 | #define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) 111 | #define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) 112 | 113 | #define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) 114 | #define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) 115 | 116 | typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); 117 | -------------------------------------------------------------------------------- /sdk/src/arch/linux/arch_linux.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | // libc dep 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | // libc++ dep 48 | #include 49 | #include 50 | 51 | // linux specific 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include "timer.h" 64 | 65 | -------------------------------------------------------------------------------- /sdk/src/arch/linux/net_serial.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "arch/linux/arch_linux.h" 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | // linux specific 42 | 43 | #include 44 | #include 45 | 46 | #include 47 | #include "hal/types.h" 48 | #include "arch/linux/net_serial.h" 49 | #include 50 | 51 | #include 52 | //__GNUC__ 53 | #if defined(__GNUC__) 54 | // for Linux extension 55 | #include 56 | #include 57 | #include 58 | extern "C" int tcflush(int fildes, int queue_selector); 59 | #else 60 | // for other standard UNIX 61 | #include 62 | #include 63 | 64 | #endif 65 | 66 | 67 | namespace rp{ namespace arch{ namespace net{ 68 | 69 | raw_serial::raw_serial() 70 | : rp::hal::serial_rxtx() 71 | , _baudrate(0) 72 | , _flags(0) 73 | , serial_fd(-1) 74 | { 75 | _init(); 76 | } 77 | 78 | raw_serial::~raw_serial() 79 | { 80 | close(); 81 | 82 | } 83 | 84 | bool raw_serial::open() 85 | { 86 | return open(_portName, _baudrate, _flags); 87 | } 88 | 89 | bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags) 90 | { 91 | strncpy(_portName, portname, sizeof(_portName)); 92 | _baudrate = baudrate; 93 | _flags = flags; 94 | return true; 95 | } 96 | 97 | bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) 98 | { 99 | if (isOpened()) close(); 100 | 101 | serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY); 102 | 103 | if (serial_fd == -1) return false; 104 | 105 | 106 | 107 | #if !defined(__GNUC__) 108 | // for standard UNIX 109 | struct termios options, oldopt; 110 | tcgetattr(serial_fd, &oldopt); 111 | bzero(&options,sizeof(struct termios)); 112 | 113 | // enable rx and tx 114 | options.c_cflag |= (CLOCAL | CREAD); 115 | 116 | _u32 termbaud = getTermBaudBitmap(baudrate); 117 | 118 | if (termbaud == (_u32)-1) { 119 | close(); 120 | return false; 121 | } 122 | cfsetispeed(&options, termbaud); 123 | cfsetospeed(&options, termbaud); 124 | 125 | options.c_cflag &= ~PARENB; //no checkbit 126 | options.c_cflag &= ~CSTOPB; //1bit stop bit 127 | options.c_cflag &= ~CRTSCTS; //no flow control 128 | 129 | options.c_cflag &= ~CSIZE; 130 | options.c_cflag |= CS8; /* Select 8 data bits */ 131 | 132 | #ifdef CNEW_RTSCTS 133 | options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control 134 | #endif 135 | 136 | options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control 137 | 138 | // raw input mode 139 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 140 | // raw output mode 141 | options.c_oflag &= ~OPOST; 142 | 143 | 144 | 145 | if (tcsetattr(serial_fd, TCSANOW, &options)) 146 | { 147 | close(); 148 | return false; 149 | } 150 | 151 | #else 152 | 153 | // using Linux extension ... 154 | struct termios2 tio; 155 | 156 | ioctl(serial_fd, TCGETS2, &tio); 157 | bzero(&tio, sizeof(struct termios2)); 158 | 159 | tio.c_cflag = BOTHER; 160 | tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake 161 | 162 | tio.c_cflag &= ~CSTOPB; //1 stop bit 163 | tio.c_cflag &= ~CRTSCTS; //No CTS 164 | tio.c_cflag &= ~PARENB; //No Parity 165 | 166 | #ifdef CNEW_RTSCTS 167 | tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control 168 | #endif 169 | 170 | tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control 171 | 172 | 173 | tio.c_cc[VMIN] = 0; //min chars to read 174 | tio.c_cc[VTIME] = 0; //time in 1/10th sec wait 175 | 176 | tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 177 | // raw output mode 178 | tio.c_oflag &= ~OPOST; 179 | 180 | tio.c_ispeed = baudrate; 181 | tio.c_ospeed = baudrate; 182 | 183 | 184 | ioctl(serial_fd, TCSETS2, &tio); 185 | 186 | #endif 187 | 188 | 189 | tcflush(serial_fd, TCIFLUSH); 190 | 191 | if (fcntl(serial_fd, F_SETFL, FNDELAY)) 192 | { 193 | close(); 194 | return false; 195 | } 196 | 197 | 198 | _is_serial_opened = true; 199 | _operation_aborted = false; 200 | 201 | //Clear the DTR bit to let the motor spin 202 | clearDTR(); 203 | do { 204 | // create self pipeline for wait cancellation 205 | if (pipe(_selfpipe) == -1) break; 206 | 207 | int flags = fcntl(_selfpipe[0], F_GETFL); 208 | if (flags == -1) 209 | break; 210 | 211 | flags |= O_NONBLOCK; /* Make read end nonblocking */ 212 | if (fcntl(_selfpipe[0], F_SETFL, flags) == -1) 213 | break; 214 | 215 | flags = fcntl(_selfpipe[1], F_GETFL); 216 | if (flags == -1) 217 | break; 218 | 219 | flags |= O_NONBLOCK; /* Make write end nonblocking */ 220 | if (fcntl(_selfpipe[1], F_SETFL, flags) == -1) 221 | break; 222 | 223 | } while (0); 224 | 225 | return true; 226 | } 227 | 228 | void raw_serial::close() 229 | { 230 | if (serial_fd != -1) 231 | ::close(serial_fd); 232 | serial_fd = -1; 233 | 234 | if (_selfpipe[0] != -1) 235 | ::close(_selfpipe[0]); 236 | 237 | if (_selfpipe[1] != -1) 238 | ::close(_selfpipe[1]); 239 | 240 | _selfpipe[0] = _selfpipe[1] = -1; 241 | 242 | _operation_aborted = false; 243 | _is_serial_opened = false; 244 | } 245 | 246 | int raw_serial::senddata(const unsigned char * data, size_t size) 247 | { 248 | // FIXME: non-block io should be used 249 | if (!isOpened()) return 0; 250 | 251 | if (data == NULL || size ==0) return 0; 252 | 253 | size_t tx_len = 0; 254 | required_tx_cnt = 0; 255 | do { 256 | int ans = ::write(serial_fd, data + tx_len, size-tx_len); 257 | 258 | if (ans == -1) return tx_len; 259 | 260 | tx_len += ans; 261 | required_tx_cnt = tx_len; 262 | }while (tx_len(serial_fd, _selfpipe[0]) + 1; 318 | 319 | /* Initialize the timeout structure */ 320 | timeout_val.tv_sec = timeout / 1000; 321 | timeout_val.tv_usec = (timeout % 1000) * 1000; 322 | 323 | if ( isOpened() ) 324 | { 325 | if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR; 326 | if (*returned_size >= data_count) 327 | { 328 | return 0; 329 | } 330 | } 331 | 332 | while ( isOpened() ) 333 | { 334 | /* Do the select */ 335 | int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val); 336 | 337 | if (n < 0) 338 | { 339 | // select error 340 | *returned_size = 0; 341 | return ANS_DEV_ERR; 342 | } 343 | else if (n == 0) 344 | { 345 | // time out 346 | *returned_size =0; 347 | return ANS_TIMEOUT; 348 | } 349 | else 350 | { 351 | if (FD_ISSET(_selfpipe[0], &input_set)) { 352 | // require aborting the current operation 353 | int ch; 354 | for (;;) { 355 | if (::read(_selfpipe[0], &ch, 1) == -1) { 356 | break; 357 | } 358 | 359 | } 360 | 361 | // treat as timeout 362 | *returned_size = 0; 363 | return ANS_TIMEOUT; 364 | } 365 | 366 | // data avaliable 367 | assert (FD_ISSET(serial_fd, &input_set)); 368 | 369 | 370 | if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR; 371 | if (*returned_size >= data_count) 372 | { 373 | return 0; 374 | } 375 | else 376 | { 377 | int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec; 378 | int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate; 379 | if (remain_timeout > expect_remain_time) 380 | usleep(expect_remain_time); 381 | } 382 | } 383 | 384 | } 385 | 386 | return ANS_DEV_ERR; 387 | } 388 | 389 | size_t raw_serial::rxqueue_count() 390 | { 391 | if ( !isOpened() ) return 0; 392 | size_t remaining; 393 | 394 | if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0; 395 | return remaining; 396 | } 397 | 398 | void raw_serial::setDTR() 399 | { 400 | if ( !isOpened() ) return; 401 | 402 | uint32_t dtr_bit = TIOCM_DTR; 403 | ioctl(serial_fd, TIOCMBIS, &dtr_bit); 404 | } 405 | 406 | void raw_serial::clearDTR() 407 | { 408 | if ( !isOpened() ) return; 409 | 410 | uint32_t dtr_bit = TIOCM_DTR; 411 | ioctl(serial_fd, TIOCMBIC, &dtr_bit); 412 | } 413 | 414 | void raw_serial::_init() 415 | { 416 | serial_fd = -1; 417 | _portName[0] = 0; 418 | required_tx_cnt = required_rx_cnt = 0; 419 | _operation_aborted = false; 420 | _selfpipe[0] = _selfpipe[1] = -1; 421 | } 422 | 423 | void raw_serial::cancelOperation() 424 | { 425 | _operation_aborted = true; 426 | if (_selfpipe[1] == -1) return; 427 | 428 | ::write(_selfpipe[1], "x", 1); 429 | } 430 | 431 | _u32 raw_serial::getTermBaudBitmap(_u32 baud) 432 | { 433 | #define BAUD_CONV( _baud_) case _baud_: return B##_baud_ 434 | switch (baud) { 435 | BAUD_CONV(1200); 436 | BAUD_CONV(1800); 437 | BAUD_CONV(2400); 438 | BAUD_CONV(4800); 439 | BAUD_CONV(9600); 440 | BAUD_CONV(19200); 441 | BAUD_CONV(38400); 442 | BAUD_CONV(57600); 443 | BAUD_CONV(115200); 444 | BAUD_CONV(230400); 445 | BAUD_CONV(460800); 446 | BAUD_CONV(500000); 447 | BAUD_CONV(576000); 448 | BAUD_CONV(921600); 449 | BAUD_CONV(1000000); 450 | BAUD_CONV(1152000); 451 | BAUD_CONV(1500000); 452 | BAUD_CONV(2000000); 453 | BAUD_CONV(2500000); 454 | BAUD_CONV(3000000); 455 | BAUD_CONV(3500000); 456 | BAUD_CONV(4000000); 457 | } 458 | return -1; 459 | } 460 | 461 | }}} //end rp::arch::net 462 | 463 | //begin rp::hal 464 | namespace rp{ namespace hal{ 465 | 466 | serial_rxtx * serial_rxtx::CreateRxTx() 467 | { 468 | return new rp::arch::net::raw_serial(); 469 | } 470 | 471 | void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx) 472 | { 473 | delete rxtx; 474 | } 475 | 476 | }} //end rp::hal 477 | -------------------------------------------------------------------------------- /sdk/src/arch/linux/net_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal/abs_rxtx.h" 38 | 39 | namespace rp{ namespace arch{ namespace net{ 40 | 41 | class raw_serial : public rp::hal::serial_rxtx 42 | { 43 | public: 44 | enum{ 45 | SERIAL_RX_BUFFER_SIZE = 512, 46 | SERIAL_TX_BUFFER_SIZE = 128, 47 | }; 48 | 49 | raw_serial(); 50 | virtual ~raw_serial(); 51 | virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0); 52 | virtual bool open(); 53 | virtual void close(); 54 | virtual void flush( _u32 flags); 55 | 56 | virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); 57 | 58 | virtual int senddata(const unsigned char * data, size_t size); 59 | virtual int recvdata(unsigned char * data, size_t size); 60 | 61 | virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); 62 | virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); 63 | 64 | virtual size_t rxqueue_count(); 65 | 66 | virtual void setDTR(); 67 | virtual void clearDTR(); 68 | 69 | _u32 getTermBaudBitmap(_u32 baud); 70 | 71 | virtual void cancelOperation(); 72 | 73 | protected: 74 | bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); 75 | void _init(); 76 | 77 | char _portName[200]; 78 | uint32_t _baudrate; 79 | uint32_t _flags; 80 | 81 | int serial_fd; 82 | 83 | size_t required_tx_cnt; 84 | size_t required_rx_cnt; 85 | 86 | int _selfpipe[2]; 87 | bool _operation_aborted; 88 | }; 89 | 90 | }}} 91 | -------------------------------------------------------------------------------- /sdk/src/arch/linux/thread.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "arch/linux/arch_linux.h" 36 | 37 | #include 38 | 39 | namespace rp{ namespace hal{ 40 | 41 | Thread Thread::create(thread_proc_t proc, void * data) 42 | { 43 | Thread newborn(proc, data); 44 | 45 | // tricky code, we assume pthread_t is not a structure but a word size value 46 | assert( sizeof(newborn._handle) >= sizeof(pthread_t)); 47 | 48 | pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data); 49 | 50 | return newborn; 51 | } 52 | 53 | u_result Thread::terminate() 54 | { 55 | if (!this->_handle) return RESULT_OK; 56 | 57 | return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; 58 | } 59 | 60 | u_result Thread::setPriority( priority_val_t p) 61 | { 62 | if (!this->_handle) return RESULT_OPERATION_FAIL; 63 | 64 | // check whether current schedule policy supports priority levels 65 | 66 | int current_policy; 67 | struct sched_param current_param; 68 | int ans; 69 | if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) 70 | { 71 | // cannot retreieve values 72 | return RESULT_OPERATION_FAIL; 73 | } 74 | 75 | //int pthread_priority = 0 ; 76 | 77 | switch(p) 78 | { 79 | case PRIORITY_REALTIME: 80 | //pthread_priority = pthread_priority_max; 81 | current_policy = SCHED_RR; 82 | break; 83 | case PRIORITY_HIGH: 84 | //pthread_priority = (pthread_priority_max + pthread_priority_min)/2; 85 | current_policy = SCHED_RR; 86 | break; 87 | case PRIORITY_NORMAL: 88 | case PRIORITY_LOW: 89 | case PRIORITY_IDLE: 90 | //pthread_priority = 0; 91 | current_policy = SCHED_OTHER; 92 | break; 93 | } 94 | 95 | current_param.__sched_priority = current_policy; 96 | if ( (ans = pthread_setschedparam( (pthread_t) this->_handle, current_policy, ¤t_param)) ) 97 | { 98 | return RESULT_OPERATION_FAIL; 99 | } 100 | return RESULT_OK; 101 | } 102 | 103 | Thread::priority_val_t Thread::getPriority() 104 | { 105 | if (!this->_handle) return PRIORITY_NORMAL; 106 | 107 | int current_policy; 108 | struct sched_param current_param; 109 | if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) 110 | { 111 | // cannot retreieve values 112 | return PRIORITY_NORMAL; 113 | } 114 | 115 | int pthread_priority_max = sched_get_priority_max(SCHED_RR); 116 | int pthread_priority_min = sched_get_priority_min(SCHED_RR); 117 | 118 | if (current_param.__sched_priority ==(pthread_priority_max )) 119 | { 120 | return PRIORITY_REALTIME; 121 | } 122 | if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2) 123 | { 124 | return PRIORITY_HIGH; 125 | } 126 | return PRIORITY_NORMAL; 127 | } 128 | 129 | u_result Thread::join(unsigned long timeout) 130 | { 131 | if (!this->_handle) return RESULT_OK; 132 | 133 | pthread_join((pthread_t)(this->_handle), NULL); 134 | return RESULT_OK; 135 | } 136 | 137 | }} 138 | -------------------------------------------------------------------------------- /sdk/src/arch/linux/timer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "arch/linux/arch_linux.h" 36 | 37 | namespace rp{ namespace arch{ 38 | _u64 rp_getus() 39 | { 40 | struct timespec t; 41 | t.tv_sec = t.tv_nsec = 0; 42 | clock_gettime(CLOCK_MONOTONIC, &t); 43 | return t.tv_sec*1000000LL + t.tv_nsec/1000; 44 | } 45 | _u32 rp_getms() 46 | { 47 | struct timespec t; 48 | t.tv_sec = t.tv_nsec = 0; 49 | clock_gettime(CLOCK_MONOTONIC, &t); 50 | return t.tv_sec*1000L + t.tv_nsec/1000000L; 51 | } 52 | }} 53 | -------------------------------------------------------------------------------- /sdk/src/arch/linux/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal/types.h" 38 | 39 | #include 40 | static inline void delay(_word_size_t ms){ 41 | while (ms>=1000){ 42 | usleep(1000*1000); 43 | ms-=1000; 44 | }; 45 | if (ms!=0) 46 | usleep(ms*1000); 47 | } 48 | 49 | // TODO: the highest timer interface should be clock_gettime 50 | namespace rp{ namespace arch{ 51 | 52 | _u64 rp_getus(); 53 | _u32 rp_getms(); 54 | 55 | }} 56 | 57 | #define getms() rp::arch::rp_getms() 58 | -------------------------------------------------------------------------------- /sdk/src/arch/macOS/arch_macOS.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | // libc dep 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | // libc++ dep 48 | #include 49 | #include 50 | 51 | 52 | // POSIX specific 53 | extern "C" { 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | } 64 | 65 | #include "arch/macOS/timer.h" 66 | 67 | -------------------------------------------------------------------------------- /sdk/src/arch/macOS/net_serial.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "arch/macOS/arch_macOS.h" 36 | #include "arch/macOS/net_serial.h" 37 | #include 38 | #include 39 | #include 40 | 41 | namespace rp{ namespace arch{ namespace net{ 42 | 43 | raw_serial::raw_serial() 44 | : rp::hal::serial_rxtx() 45 | , _baudrate(0) 46 | , _flags(0) 47 | , serial_fd(-1) 48 | { 49 | _init(); 50 | } 51 | 52 | raw_serial::~raw_serial() 53 | { 54 | close(); 55 | 56 | } 57 | 58 | bool raw_serial::open() 59 | { 60 | return open(_portName, _baudrate, _flags); 61 | } 62 | 63 | bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags) 64 | { 65 | strncpy(_portName, portname, sizeof(_portName)); 66 | _baudrate = baudrate; 67 | _flags = flags; 68 | return true; 69 | } 70 | 71 | bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) 72 | { 73 | if (isOpened()) close(); 74 | 75 | serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY); 76 | 77 | if (serial_fd == -1) return false; 78 | 79 | struct termios options, oldopt; 80 | tcgetattr(serial_fd, &oldopt); 81 | bzero(&options,sizeof(struct termios)); 82 | 83 | cfsetspeed(&options, B19200); 84 | 85 | // enable rx and tx 86 | options.c_cflag |= (CLOCAL | CREAD); 87 | 88 | 89 | options.c_cflag &= ~PARENB; //no checkbit 90 | options.c_cflag &= ~CSTOPB; //1bit stop bit 91 | 92 | options.c_cflag &= ~CSIZE; 93 | options.c_cflag |= CS8; /* Select 8 data bits */ 94 | 95 | #ifdef CNEW_RTSCTS 96 | options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control 97 | #endif 98 | 99 | options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control 100 | 101 | // raw input mode 102 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 103 | // raw output mode 104 | options.c_oflag &= ~OPOST; 105 | 106 | tcflush(serial_fd,TCIFLUSH); 107 | 108 | if (tcsetattr(serial_fd, TCSANOW, &options)) 109 | { 110 | close(); 111 | return false; 112 | } 113 | 114 | printf("Setting serial port baudrate...\n"); 115 | 116 | speed_t speed = (speed_t)baudrate; 117 | if (ioctl(serial_fd, IOSSIOSPEED, &speed)== -1) { 118 | printf("Error calling ioctl(..., IOSSIOSPEED, ...) %s - %s(%d).\n", 119 | portname, strerror(errno), errno); 120 | close(); 121 | return false; 122 | } 123 | 124 | _is_serial_opened = true; 125 | 126 | //Clear the DTR bit to let the motor spin 127 | clearDTR(); 128 | 129 | return true; 130 | } 131 | 132 | void raw_serial::close() 133 | { 134 | if (serial_fd != -1) 135 | ::close(serial_fd); 136 | serial_fd = -1; 137 | 138 | _is_serial_opened = false; 139 | } 140 | 141 | int raw_serial::senddata(const unsigned char * data, _word_size_t size) 142 | { 143 | // FIXME: non-block io should be used 144 | if (!isOpened()) return 0; 145 | 146 | if (data == NULL || size ==0) return 0; 147 | 148 | size_t tx_len = 0; 149 | required_tx_cnt = 0; 150 | do { 151 | int ans = ::write(serial_fd, data + tx_len, size-tx_len); 152 | 153 | if (ans == -1) return tx_len; 154 | 155 | tx_len += ans; 156 | required_tx_cnt = tx_len; 157 | }while (tx_len= data_count) 219 | { 220 | return 0; 221 | } 222 | } 223 | 224 | while ( isOpened() ) 225 | { 226 | /* Do the select */ 227 | int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val); 228 | 229 | if (n < 0) 230 | { 231 | // select error 232 | return ANS_DEV_ERR; 233 | } 234 | else if (n == 0) 235 | { 236 | // time out 237 | return ANS_TIMEOUT; 238 | } 239 | else 240 | { 241 | // data avaliable 242 | assert (FD_ISSET(serial_fd, &input_set)); 243 | 244 | 245 | if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR; 246 | if (*returned_size >= data_count) 247 | { 248 | return 0; 249 | } 250 | else 251 | { 252 | int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec; 253 | int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate; 254 | if (remain_timeout > expect_remain_time) 255 | usleep(expect_remain_time); 256 | } 257 | } 258 | 259 | } 260 | return ANS_DEV_ERR; 261 | } 262 | 263 | size_t raw_serial::rxqueue_count() 264 | { 265 | if ( !isOpened() ) return 0; 266 | size_t remaining; 267 | 268 | if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0; 269 | return remaining; 270 | } 271 | 272 | void raw_serial::setDTR() 273 | { 274 | if ( !isOpened() ) return; 275 | 276 | uint32_t dtr_bit = TIOCM_DTR; 277 | ioctl(serial_fd, TIOCMBIS, &dtr_bit); 278 | } 279 | 280 | void raw_serial::clearDTR() 281 | { 282 | if ( !isOpened() ) return; 283 | 284 | uint32_t dtr_bit = TIOCM_DTR; 285 | ioctl(serial_fd, TIOCMBIC, &dtr_bit); 286 | } 287 | 288 | void raw_serial::_init() 289 | { 290 | serial_fd = 0; 291 | _portName[0] = 0; 292 | required_tx_cnt = required_rx_cnt = 0; 293 | } 294 | 295 | 296 | 297 | _u32 raw_serial::getTermBaudBitmap(_u32 baud) 298 | { 299 | #define BAUD_CONV(_baud_) case _baud_: return _baud_ 300 | switch (baud) 301 | { 302 | BAUD_CONV(1200); 303 | BAUD_CONV(1800); 304 | BAUD_CONV(2400); 305 | BAUD_CONV(4800); 306 | BAUD_CONV(9600); 307 | BAUD_CONV(19200); 308 | BAUD_CONV(38400); 309 | BAUD_CONV(57600); 310 | BAUD_CONV(115200); 311 | BAUD_CONV(230400); 312 | BAUD_CONV(460800); 313 | BAUD_CONV(500000); 314 | BAUD_CONV(576000); 315 | BAUD_CONV(921600); 316 | BAUD_CONV(1000000); 317 | BAUD_CONV(1152000); 318 | BAUD_CONV(1500000); 319 | BAUD_CONV(2000000); 320 | BAUD_CONV(2500000); 321 | BAUD_CONV(3000000); 322 | BAUD_CONV(3500000); 323 | BAUD_CONV(4000000); 324 | } 325 | return -1; 326 | } 327 | 328 | }}} //end rp::arch::net 329 | 330 | 331 | 332 | //begin rp::hal 333 | namespace rp{ namespace hal{ 334 | 335 | serial_rxtx * serial_rxtx::CreateRxTx() 336 | { 337 | return new rp::arch::net::raw_serial(); 338 | } 339 | 340 | void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx) 341 | { 342 | delete rxtx; 343 | } 344 | 345 | }} //end rp::hal 346 | -------------------------------------------------------------------------------- /sdk/src/arch/macOS/net_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal/abs_rxtx.h" 38 | 39 | namespace rp{ namespace arch{ namespace net{ 40 | 41 | class raw_serial : public rp::hal::serial_rxtx 42 | { 43 | public: 44 | enum{ 45 | SERIAL_RX_BUFFER_SIZE = 512, 46 | SERIAL_TX_BUFFER_SIZE = 128, 47 | }; 48 | 49 | raw_serial(); 50 | virtual ~raw_serial(); 51 | virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0); 52 | virtual bool open(); 53 | virtual void close(); 54 | virtual void flush( _u32 flags); 55 | 56 | virtual int waitfordata(_word_size_t data_count,_u32 timeout = -1, _word_size_t * returned_size = NULL); 57 | 58 | virtual int senddata(const unsigned char * data, _word_size_t size); 59 | virtual int recvdata(unsigned char * data, _word_size_t size); 60 | 61 | virtual int waitforsent(_u32 timeout = -1, _word_size_t * returned_size = NULL); 62 | virtual int waitforrecv(_u32 timeout = -1, _word_size_t * returned_size = NULL); 63 | 64 | virtual size_t rxqueue_count(); 65 | 66 | virtual void setDTR(); 67 | virtual void clearDTR(); 68 | 69 | _u32 getTermBaudBitmap(_u32 baud); 70 | protected: 71 | bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); 72 | void _init(); 73 | 74 | char _portName[200]; 75 | uint32_t _baudrate; 76 | uint32_t _flags; 77 | 78 | int serial_fd; 79 | 80 | size_t required_tx_cnt; 81 | size_t required_rx_cnt; 82 | }; 83 | 84 | }}} 85 | -------------------------------------------------------------------------------- /sdk/src/arch/macOS/thread.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "arch/macOS/arch_macOS.h" 36 | 37 | namespace rp{ namespace hal{ 38 | 39 | Thread Thread::create(thread_proc_t proc, void * data) 40 | { 41 | Thread newborn(proc, data); 42 | 43 | // tricky code, we assume pthread_t is not a structure but a word size value 44 | assert( sizeof(newborn._handle) >= sizeof(pthread_t)); 45 | 46 | pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data); 47 | 48 | return newborn; 49 | } 50 | 51 | u_result Thread::terminate() 52 | { 53 | if (!this->_handle) return RESULT_OK; 54 | 55 | // return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; 56 | return RESULT_OK; 57 | } 58 | 59 | u_result Thread::setPriority( priority_val_t p) 60 | { 61 | if (!this->_handle) return RESULT_OPERATION_FAIL; 62 | // simply ignore this request 63 | return RESULT_OK; 64 | } 65 | 66 | Thread::priority_val_t Thread::getPriority() 67 | { 68 | return PRIORITY_NORMAL; 69 | } 70 | 71 | u_result Thread::join(unsigned long timeout) 72 | { 73 | if (!this->_handle) return RESULT_OK; 74 | 75 | pthread_join((pthread_t)(this->_handle), NULL); 76 | return RESULT_OK; 77 | } 78 | 79 | }} 80 | -------------------------------------------------------------------------------- /sdk/src/arch/macOS/timer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "arch/macOS/arch_macOS.h" 36 | 37 | 38 | namespace rp{ namespace arch{ 39 | _u64 getus() 40 | { 41 | timeval now; 42 | gettimeofday(&now,NULL); 43 | return now.tv_sec*1000000 + now.tv_usec; 44 | } 45 | 46 | _u32 rp_getms() 47 | { 48 | timeval now; 49 | gettimeofday(&now,NULL); 50 | return now.tv_sec*1000L + now.tv_usec/1000L; 51 | } 52 | 53 | }} 54 | -------------------------------------------------------------------------------- /sdk/src/arch/macOS/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "rptypes.h" 38 | 39 | #include 40 | static inline void delay(_word_size_t ms){ 41 | while (ms>=1000){ 42 | usleep(1000*1000); 43 | ms-=1000; 44 | }; 45 | if (ms!=0) 46 | usleep(ms*1000); 47 | } 48 | 49 | // TODO: the highest timer interface should be clock_gettime 50 | namespace rp{ namespace arch{ 51 | 52 | _u64 rp_getus(); 53 | _u32 rp_getms(); 54 | 55 | }} 56 | 57 | #define getms() rp::arch::rp_getms() 58 | -------------------------------------------------------------------------------- /sdk/src/arch/win32/arch_win32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #pragma warning (disable: 4996) 38 | #define _CRT_SECURE_NO_WARNINGS 39 | 40 | #ifndef WINVER 41 | #define WINVER 0x0500 42 | #endif 43 | 44 | #ifndef _WIN32_WINNT 45 | #define _WIN32_WINNT 0x0501 46 | #endif 47 | 48 | 49 | #ifndef _WIN32_IE 50 | #define _WIN32_IE 0x0501 51 | #endif 52 | 53 | #ifndef _RICHEDIT_VER 54 | #define _RICHEDIT_VER 0x0200 55 | #endif 56 | 57 | 58 | #include 59 | #include 60 | #include 61 | #include //for memcpy etc.. 62 | #include 63 | #include 64 | 65 | 66 | #include "timer.h" 67 | -------------------------------------------------------------------------------- /sdk/src/arch/win32/net_serial.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "sdkcommon.h" 36 | #include "net_serial.h" 37 | 38 | namespace rp{ namespace arch{ namespace net{ 39 | 40 | raw_serial::raw_serial() 41 | : rp::hal::serial_rxtx() 42 | , _serial_handle(NULL) 43 | , _baudrate(0) 44 | , _flags(0) 45 | { 46 | _init(); 47 | } 48 | 49 | raw_serial::~raw_serial() 50 | { 51 | close(); 52 | 53 | CloseHandle(_ro.hEvent); 54 | CloseHandle(_wo.hEvent); 55 | CloseHandle(_wait_o.hEvent); 56 | } 57 | 58 | bool raw_serial::open() 59 | { 60 | return open(_portName, _baudrate, _flags); 61 | } 62 | 63 | bool raw_serial::bind(const char * portname, _u32 baudrate, _u32 flags) 64 | { 65 | strncpy(_portName, portname, sizeof(_portName)); 66 | _baudrate = baudrate; 67 | _flags = flags; 68 | return true; 69 | } 70 | 71 | bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags) 72 | { 73 | if (isOpened()) close(); 74 | 75 | _serial_handle = CreateFile( 76 | portname, 77 | GENERIC_READ | GENERIC_WRITE, 78 | 0, 79 | NULL, 80 | OPEN_EXISTING, 81 | FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, 82 | NULL 83 | ); 84 | 85 | if (_serial_handle == INVALID_HANDLE_VALUE) return false; 86 | 87 | if (!SetupComm(_serial_handle, SERIAL_RX_BUFFER_SIZE, SERIAL_TX_BUFFER_SIZE)) 88 | { 89 | close(); 90 | return false; 91 | } 92 | 93 | _dcb.BaudRate = baudrate; 94 | _dcb.ByteSize = 8; 95 | _dcb.Parity = NOPARITY; 96 | _dcb.StopBits = ONESTOPBIT; 97 | _dcb.fDtrControl = DTR_CONTROL_ENABLE; 98 | 99 | if (!SetCommState(_serial_handle, &_dcb)) 100 | { 101 | close(); 102 | return false; 103 | } 104 | 105 | if (!SetCommTimeouts(_serial_handle, &_co)) 106 | { 107 | close(); 108 | return false; 109 | } 110 | 111 | if (!SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR )) 112 | { 113 | close(); 114 | return false; 115 | } 116 | 117 | if (!PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR )) 118 | { 119 | close(); 120 | return false; 121 | } 122 | 123 | Sleep(30); 124 | _is_serial_opened = true; 125 | 126 | //Clear the DTR bit set DTR=high 127 | clearDTR(); 128 | 129 | return true; 130 | } 131 | 132 | void raw_serial::close() 133 | { 134 | SetCommMask(_serial_handle, 0); 135 | ResetEvent(_wait_o.hEvent); 136 | 137 | CloseHandle(_serial_handle); 138 | _serial_handle = INVALID_HANDLE_VALUE; 139 | 140 | _is_serial_opened = false; 141 | } 142 | 143 | int raw_serial::senddata(const unsigned char * data, size_t size) 144 | { 145 | DWORD error; 146 | DWORD w_len = 0, o_len = -1; 147 | if (!isOpened()) return ANS_DEV_ERR; 148 | 149 | if (data == NULL || size ==0) return 0; 150 | 151 | if(ClearCommError(_serial_handle, &error, NULL) && error > 0) 152 | PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_TXCLEAR); 153 | 154 | if(!WriteFile(_serial_handle, data, size, &w_len, &_wo)) 155 | if(GetLastError() != ERROR_IO_PENDING) 156 | w_len = ANS_DEV_ERR; 157 | 158 | return w_len; 159 | } 160 | 161 | int raw_serial::recvdata(unsigned char * data, size_t size) 162 | { 163 | if (!isOpened()) return 0; 164 | DWORD r_len = 0; 165 | 166 | 167 | if(!ReadFile(_serial_handle, data, size, &r_len, &_ro)) 168 | { 169 | if(GetLastError() == ERROR_IO_PENDING) 170 | { 171 | if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) 172 | { 173 | if(GetLastError() != ERROR_IO_INCOMPLETE) 174 | r_len = 0; 175 | } 176 | } 177 | else 178 | r_len = 0; 179 | } 180 | 181 | return r_len; 182 | } 183 | 184 | void raw_serial::flush( _u32 flags) 185 | { 186 | PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR ); 187 | } 188 | 189 | int raw_serial::waitforsent(_u32 timeout, size_t * returned_size) 190 | { 191 | if (!isOpened() ) return ANS_DEV_ERR; 192 | DWORD w_len = 0; 193 | _word_size_t ans =0; 194 | 195 | if (WaitForSingleObject(_wo.hEvent, timeout) == WAIT_TIMEOUT) 196 | { 197 | ans = ANS_TIMEOUT; 198 | goto _final; 199 | } 200 | if(!GetOverlappedResult(_serial_handle, &_wo, &w_len, FALSE)) 201 | { 202 | ans = ANS_DEV_ERR; 203 | } 204 | _final: 205 | if (returned_size) *returned_size = w_len; 206 | return ans; 207 | } 208 | 209 | int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size) 210 | { 211 | if (!isOpened() ) return -1; 212 | DWORD r_len = 0; 213 | _word_size_t ans =0; 214 | 215 | if (WaitForSingleObject(_ro.hEvent, timeout) == WAIT_TIMEOUT) 216 | { 217 | ans = ANS_TIMEOUT; 218 | } 219 | if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) 220 | { 221 | ans = ANS_DEV_ERR; 222 | } 223 | if (returned_size) *returned_size = r_len; 224 | return ans; 225 | } 226 | 227 | int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size) 228 | { 229 | COMSTAT stat; 230 | DWORD error; 231 | DWORD msk,length; 232 | size_t dummy_length; 233 | 234 | if (returned_size==NULL) returned_size=(size_t *)&dummy_length; 235 | 236 | 237 | if ( isOpened()) { 238 | size_t rxqueue_remaining = rxqueue_count(); 239 | if (rxqueue_remaining >= data_count) { 240 | *returned_size = rxqueue_remaining; 241 | return 0; 242 | } 243 | } 244 | 245 | while ( isOpened() ) 246 | { 247 | msk = 0; 248 | SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR ); 249 | if(!WaitCommEvent(_serial_handle, &msk, &_wait_o)) 250 | { 251 | if(GetLastError() == ERROR_IO_PENDING) 252 | { 253 | if (WaitForSingleObject(_wait_o.hEvent, timeout) == WAIT_TIMEOUT) 254 | { 255 | *returned_size =0; 256 | return ANS_TIMEOUT; 257 | } 258 | 259 | GetOverlappedResult(_serial_handle, &_wait_o, &length, TRUE); 260 | 261 | ::ResetEvent(_wait_o.hEvent); 262 | }else 263 | { 264 | ClearCommError(_serial_handle, &error, &stat); 265 | *returned_size = stat.cbInQue; 266 | return ANS_DEV_ERR; 267 | } 268 | } 269 | 270 | if(msk & EV_ERR){ 271 | // FIXME: may cause problem here 272 | ClearCommError(_serial_handle, &error, &stat); 273 | } 274 | 275 | if(msk & EV_RXCHAR){ 276 | ClearCommError(_serial_handle, &error, &stat); 277 | if(stat.cbInQue >= data_count) 278 | { 279 | *returned_size = stat.cbInQue; 280 | return 0; 281 | } 282 | } 283 | } 284 | *returned_size=0; 285 | return ANS_DEV_ERR; 286 | } 287 | 288 | size_t raw_serial::rxqueue_count() 289 | { 290 | if ( !isOpened() ) return 0; 291 | COMSTAT com_stat; 292 | DWORD error; 293 | DWORD r_len = 0; 294 | 295 | if(ClearCommError(_serial_handle, &error, &com_stat) && error > 0) 296 | { 297 | PurgeComm(_serial_handle, PURGE_RXABORT | PURGE_RXCLEAR); 298 | return 0; 299 | } 300 | return com_stat.cbInQue; 301 | } 302 | 303 | void raw_serial::setDTR() 304 | { 305 | if ( !isOpened() ) return; 306 | 307 | EscapeCommFunction(_serial_handle, SETDTR); 308 | } 309 | 310 | void raw_serial::clearDTR() 311 | { 312 | if ( !isOpened() ) return; 313 | 314 | EscapeCommFunction(_serial_handle, CLRDTR); 315 | } 316 | 317 | 318 | void raw_serial::_init() 319 | { 320 | memset(&_dcb, 0, sizeof(_dcb)); 321 | _dcb.DCBlength = sizeof(_dcb); 322 | _serial_handle = INVALID_HANDLE_VALUE; 323 | memset(&_co, 0, sizeof(_co)); 324 | _co.ReadIntervalTimeout = 0; 325 | _co.ReadTotalTimeoutMultiplier = 0; 326 | _co.ReadTotalTimeoutConstant = 0; 327 | _co.WriteTotalTimeoutMultiplier = 0; 328 | _co.WriteTotalTimeoutConstant = 0; 329 | 330 | memset(&_ro, 0, sizeof(_ro)); 331 | memset(&_wo, 0, sizeof(_wo)); 332 | memset(&_wait_o, 0, sizeof(_wait_o)); 333 | 334 | _ro.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); 335 | _wo.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); 336 | _wait_o.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); 337 | 338 | _portName[0] = 0; 339 | } 340 | 341 | }}} //end rp::arch::net 342 | 343 | 344 | //begin rp::hal 345 | namespace rp{ namespace hal{ 346 | 347 | serial_rxtx * serial_rxtx::CreateRxTx() 348 | { 349 | return new rp::arch::net::raw_serial(); 350 | } 351 | 352 | void serial_rxtx::ReleaseRxTx( serial_rxtx * rxtx) 353 | { 354 | delete rxtx; 355 | } 356 | 357 | 358 | }} //end rp::hal 359 | -------------------------------------------------------------------------------- /sdk/src/arch/win32/net_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal/abs_rxtx.h" 38 | 39 | namespace rp{ namespace arch{ namespace net{ 40 | 41 | class raw_serial : public rp::hal::serial_rxtx 42 | { 43 | public: 44 | enum{ 45 | SERIAL_RX_BUFFER_SIZE = 512, 46 | SERIAL_TX_BUFFER_SIZE = 128, 47 | SERIAL_RX_TIMEOUT = 2000, 48 | SERIAL_TX_TIMEOUT = 2000, 49 | }; 50 | 51 | raw_serial(); 52 | virtual ~raw_serial(); 53 | virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0); 54 | virtual bool open(); 55 | virtual void close(); 56 | virtual void flush( _u32 flags); 57 | 58 | virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); 59 | 60 | virtual int senddata(const unsigned char * data, size_t size); 61 | virtual int recvdata(unsigned char * data, size_t size); 62 | 63 | virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); 64 | virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); 65 | 66 | virtual size_t rxqueue_count(); 67 | 68 | virtual void setDTR(); 69 | virtual void clearDTR(); 70 | 71 | protected: 72 | bool open(const char * portname, _u32 baudrate, _u32 flags); 73 | void _init(); 74 | 75 | char _portName[20]; 76 | uint32_t _baudrate; 77 | uint32_t _flags; 78 | 79 | OVERLAPPED _ro, _wo; 80 | OVERLAPPED _wait_o; 81 | volatile HANDLE _serial_handle; 82 | DCB _dcb; 83 | COMMTIMEOUTS _co; 84 | }; 85 | 86 | }}} 87 | -------------------------------------------------------------------------------- /sdk/src/arch/win32/timer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "sdkcommon.h" 36 | #include 37 | #pragma comment(lib, "Winmm.lib") 38 | 39 | namespace rp{ namespace arch{ 40 | 41 | static LARGE_INTEGER _current_freq; 42 | 43 | void HPtimer_reset() 44 | { 45 | BOOL ans=QueryPerformanceFrequency(&_current_freq); 46 | _current_freq.QuadPart/=1000; 47 | } 48 | 49 | _u32 getHDTimer() 50 | { 51 | LARGE_INTEGER current; 52 | QueryPerformanceCounter(¤t); 53 | 54 | return (_u32)(current.QuadPart/_current_freq.QuadPart); 55 | } 56 | 57 | BEGIN_STATIC_CODE(timer_cailb) 58 | { 59 | HPtimer_reset(); 60 | }END_STATIC_CODE(timer_cailb) 61 | 62 | }} 63 | -------------------------------------------------------------------------------- /sdk/src/arch/win32/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal/types.h" 38 | 39 | #define delay(x) ::Sleep(x) 40 | 41 | namespace rp{ namespace arch{ 42 | void HPtimer_reset(); 43 | _u32 getHDTimer(); 44 | }} 45 | 46 | #define getms() rp::arch::getHDTimer() 47 | 48 | -------------------------------------------------------------------------------- /sdk/src/arch/win32/winthread.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "sdkcommon.h" 36 | #include 37 | 38 | namespace rp{ namespace hal{ 39 | 40 | Thread Thread::create(thread_proc_t proc, void * data) 41 | { 42 | Thread newborn(proc, data); 43 | 44 | newborn._handle = (_word_size_t)( 45 | _beginthreadex(NULL, 0, (unsigned int (_stdcall * )( void * ))proc, 46 | data, 0, NULL)); 47 | return newborn; 48 | } 49 | 50 | u_result Thread::terminate() 51 | { 52 | if (!this->_handle) return RESULT_OK; 53 | if (TerminateThread( reinterpret_cast(this->_handle), -1)) 54 | { 55 | CloseHandle(reinterpret_cast(this->_handle)); 56 | this->_handle = NULL; 57 | return RESULT_OK; 58 | }else 59 | { 60 | return RESULT_OPERATION_FAIL; 61 | } 62 | } 63 | 64 | u_result Thread::setPriority( priority_val_t p) 65 | { 66 | if (!this->_handle) return RESULT_OPERATION_FAIL; 67 | 68 | int win_priority = THREAD_PRIORITY_NORMAL; 69 | switch(p) 70 | { 71 | case PRIORITY_REALTIME: 72 | win_priority = THREAD_PRIORITY_TIME_CRITICAL; 73 | break; 74 | case PRIORITY_HIGH: 75 | win_priority = THREAD_PRIORITY_HIGHEST; 76 | break; 77 | case PRIORITY_NORMAL: 78 | win_priority = THREAD_PRIORITY_NORMAL; 79 | break; 80 | case PRIORITY_LOW: 81 | win_priority = THREAD_PRIORITY_LOWEST; 82 | break; 83 | case PRIORITY_IDLE: 84 | win_priority = THREAD_PRIORITY_IDLE; 85 | break; 86 | } 87 | 88 | if (SetThreadPriority(reinterpret_cast(this->_handle), win_priority)) 89 | { 90 | return RESULT_OK; 91 | } 92 | return RESULT_OPERATION_FAIL; 93 | } 94 | 95 | Thread::priority_val_t Thread::getPriority() 96 | { 97 | if (!this->_handle) return PRIORITY_NORMAL; 98 | int win_priority = ::GetThreadPriority(reinterpret_cast(this->_handle)); 99 | 100 | if (win_priority == THREAD_PRIORITY_ERROR_RETURN) 101 | { 102 | return PRIORITY_NORMAL; 103 | } 104 | 105 | if (win_priority >= THREAD_PRIORITY_TIME_CRITICAL ) 106 | { 107 | return PRIORITY_REALTIME; 108 | } 109 | else if (win_priority=THREAD_PRIORITY_ABOVE_NORMAL) 110 | { 111 | return PRIORITY_HIGH; 112 | } 113 | else if (win_priorityTHREAD_PRIORITY_BELOW_NORMAL) 114 | { 115 | return PRIORITY_NORMAL; 116 | }else if (win_priority<=THREAD_PRIORITY_BELOW_NORMAL && win_priority>THREAD_PRIORITY_IDLE) 117 | { 118 | return PRIORITY_LOW; 119 | }else if (win_priority<=THREAD_PRIORITY_IDLE) 120 | { 121 | return PRIORITY_IDLE; 122 | } 123 | return PRIORITY_NORMAL; 124 | } 125 | 126 | u_result Thread::join(unsigned long timeout) 127 | { 128 | if (!this->_handle) return RESULT_OK; 129 | switch ( WaitForSingleObject(reinterpret_cast(this->_handle), timeout)) 130 | { 131 | case WAIT_OBJECT_0: 132 | CloseHandle(reinterpret_cast(this->_handle)); 133 | this->_handle = NULL; 134 | return RESULT_OK; 135 | case WAIT_ABANDONED: 136 | return RESULT_OPERATION_FAIL; 137 | case WAIT_TIMEOUT: 138 | return RESULT_OPERATION_TIMEOUT; 139 | } 140 | 141 | return RESULT_OK; 142 | } 143 | 144 | }} 145 | -------------------------------------------------------------------------------- /sdk/src/hal/abs_rxtx.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal/types.h" 38 | 39 | namespace rp{ namespace hal{ 40 | 41 | class serial_rxtx 42 | { 43 | public: 44 | enum{ 45 | ANS_OK = 0, 46 | ANS_TIMEOUT = -1, 47 | ANS_DEV_ERR = -2, 48 | }; 49 | 50 | static serial_rxtx * CreateRxTx(); 51 | static void ReleaseRxTx( serial_rxtx * ); 52 | 53 | serial_rxtx():_is_serial_opened(false){} 54 | virtual ~serial_rxtx(){} 55 | 56 | virtual void flush( _u32 flags) = 0; 57 | 58 | virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0) = 0; 59 | virtual bool open() = 0; 60 | virtual void close() = 0; 61 | 62 | virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0; 63 | 64 | virtual int senddata(const unsigned char * data, size_t size) = 0; 65 | virtual int recvdata(unsigned char * data, size_t size) = 0; 66 | 67 | virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL) = 0; 68 | virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL) = 0; 69 | 70 | virtual size_t rxqueue_count() = 0; 71 | 72 | virtual void setDTR() = 0; 73 | virtual void clearDTR() = 0; 74 | virtual void cancelOperation() {} 75 | 76 | virtual bool isOpened() 77 | { 78 | return _is_serial_opened; 79 | } 80 | 81 | protected: 82 | volatile bool _is_serial_opened; 83 | }; 84 | 85 | }} 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /sdk/src/hal/assert.h: -------------------------------------------------------------------------------- 1 | #ifndef _INFRA_HAL_ASSERT_H 2 | #define _INFRA_HAL_ASSERT_H 3 | 4 | #ifdef WIN32 5 | #include 6 | #ifndef assert 7 | #define assert(x) _ASSERT(x) 8 | #endif 9 | #elif defined(_AVR_) 10 | #define assert(x) 11 | #elif defined(__GNUC__) 12 | #ifndef assert 13 | #define assert(x) 14 | #endif 15 | #else 16 | #error assert.h cannot identify your platform 17 | #endif 18 | #endif 19 | -------------------------------------------------------------------------------- /sdk/src/hal/byteops.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RoboPeak Project 3 | * Copyright 2009 - 2013 4 | * 5 | * RPOS - Byte Operations 6 | * 7 | */ 8 | 9 | #pragma once 10 | 11 | // byte swapping operations for compiling time 12 | 13 | #define __static_byteswap_16(x) ((_u16)( \ 14 | (((_u16)(x) & (_u16)0x00FFU) << 8) | \ 15 | (((_u16)(x) & (_u16)0xFF00U) >> 8))) 16 | 17 | #define __static_byteswap_32(x) ((_u32)( \ 18 | (((_u32)(x) & (_u32)0x000000FFUL) << 24) | \ 19 | (((_u32)(x) & (_u32)0x0000FF00UL) << 8) | \ 20 | (((_u32)(x) & (_u32)0x00FF0000UL) >> 8) | \ 21 | (((_u32)(x) & (_u32)0xFF000000UL) >> 24))) 22 | 23 | #define __static_byteswap_64(x) ((_u64)( \ 24 | (((_u64)(x) & (_u64)0x00000000000000ffULL) << 56) | \ 25 | (((_u64)(x) & (_u64)0x000000000000ff00ULL) << 40) | \ 26 | (((_u64)(x) & (_u64)0x0000000000ff0000ULL) << 24) | \ 27 | (((_u64)(x) & (_u64)0x00000000ff000000ULL) << 8) | \ 28 | (((_u64)(x) & (_u64)0x000000ff00000000ULL) >> 8) | \ 29 | (((_u64)(x) & (_u64)0x0000ff0000000000ULL) >> 24) | \ 30 | (((_u64)(x) & (_u64)0x00ff000000000000ULL) >> 40) | \ 31 | (((_u64)(x) & (_u64)0xff00000000000000ULL) >> 56))) 32 | 33 | 34 | #define __fast_swap(a, b) do { (a) ^= (b); (b) ^= (a); (a) ^= (b); } while(0) 35 | 36 | 37 | static inline _u16 __byteswap_16(_u16 x) 38 | { 39 | #ifdef __arch_byteswap_16 40 | return __arch_byteswap_16(x); 41 | #else 42 | return __static_byteswap_16(x); 43 | #endif 44 | } 45 | 46 | static inline _u32 __byteswap_32(_u32 x) 47 | { 48 | #ifdef __arch_byteswap_32 49 | return __arch_byteswap_32(x); 50 | #else 51 | return __static_byteswap_32(x); 52 | #endif 53 | } 54 | 55 | static inline _u64 __byteswap_64(_u64 x) 56 | { 57 | #ifdef __arch_byteswap_64 58 | return __arch_byteswap_64(x); 59 | #else 60 | return __static_byteswap_64(x); 61 | #endif 62 | } 63 | 64 | 65 | #ifdef float 66 | static inline float __byteswap_float(float x) 67 | { 68 | #ifdef __arch_byteswap_float 69 | return __arch_byteswap_float(x); 70 | #else 71 | _u8 * raw = (_u8 *)&x; 72 | __fast_swap(raw[0], raw[3]); 73 | __fast_swap(raw[1], raw[2]); 74 | return x; 75 | #endif 76 | } 77 | #endif 78 | 79 | 80 | #ifdef double 81 | static inline double __byteswap_double(double x) 82 | { 83 | #ifdef __arch_byteswap_double 84 | return __arch_byteswap_double(x); 85 | #else 86 | _u8 * raw = (_u8 *)&x; 87 | __fast_swap(raw[0], raw[7]); 88 | __fast_swap(raw[1], raw[6]); 89 | __fast_swap(raw[2], raw[5]); 90 | __fast_swap(raw[3], raw[4]); 91 | return x; 92 | #endif 93 | } 94 | #endif 95 | -------------------------------------------------------------------------------- /sdk/src/hal/event.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | namespace rp{ namespace hal{ 37 | 38 | class Event 39 | { 40 | public: 41 | 42 | enum 43 | { 44 | EVENT_OK = 1, 45 | EVENT_TIMEOUT = -1, 46 | EVENT_FAILED = 0, 47 | }; 48 | 49 | Event(bool isAutoReset = true, bool isSignal = false) 50 | #ifdef _WIN32 51 | : _event(NULL) 52 | #else 53 | : _is_signalled(isSignal) 54 | , _isAutoReset(isAutoReset) 55 | #endif 56 | { 57 | #ifdef _WIN32 58 | _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); 59 | #else 60 | pthread_mutex_init(&_cond_locker, NULL); 61 | pthread_cond_init(&_cond_var, NULL); 62 | #endif 63 | } 64 | 65 | ~ Event() 66 | { 67 | release(); 68 | } 69 | 70 | void set( bool isSignal = true ) 71 | { 72 | if (isSignal){ 73 | #ifdef _WIN32 74 | SetEvent(_event); 75 | #else 76 | pthread_mutex_lock(&_cond_locker); 77 | 78 | if ( _is_signalled == false ) 79 | { 80 | _is_signalled = true; 81 | pthread_cond_signal(&_cond_var); 82 | } 83 | pthread_mutex_unlock(&_cond_locker); 84 | #endif 85 | } 86 | else 87 | { 88 | #ifdef _WIN32 89 | ResetEvent(_event); 90 | #else 91 | pthread_mutex_lock(&_cond_locker); 92 | _is_signalled = false; 93 | pthread_mutex_unlock(&_cond_locker); 94 | #endif 95 | } 96 | } 97 | 98 | unsigned long wait( unsigned long timeout = 0xFFFFFFFF ) 99 | { 100 | #ifdef _WIN32 101 | switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) 102 | { 103 | case WAIT_FAILED: 104 | return EVENT_FAILED; 105 | case WAIT_OBJECT_0: 106 | return EVENT_OK; 107 | case WAIT_TIMEOUT: 108 | return EVENT_TIMEOUT; 109 | } 110 | return EVENT_OK; 111 | #else 112 | unsigned long ans = EVENT_OK; 113 | pthread_mutex_lock( &_cond_locker ); 114 | 115 | if ( !_is_signalled ) 116 | { 117 | 118 | if (timeout == 0xFFFFFFFF){ 119 | pthread_cond_wait(&_cond_var,&_cond_locker); 120 | }else 121 | { 122 | timespec wait_time; 123 | timeval now; 124 | gettimeofday(&now,NULL); 125 | 126 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 127 | wait_time.tv_nsec = (timeout%1000)*1000000ULL + now.tv_usec*1000; 128 | 129 | if (wait_time.tv_nsec >= 1000000000) 130 | { 131 | ++wait_time.tv_sec; 132 | wait_time.tv_nsec -= 1000000000; 133 | } 134 | switch (pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time)) 135 | { 136 | case 0: 137 | // signalled 138 | break; 139 | case ETIMEDOUT: 140 | // time up 141 | ans = EVENT_TIMEOUT; 142 | goto _final; 143 | break; 144 | default: 145 | ans = EVENT_FAILED; 146 | goto _final; 147 | } 148 | 149 | } 150 | } 151 | 152 | assert(_is_signalled); 153 | 154 | if ( _isAutoReset ) 155 | { 156 | _is_signalled = false; 157 | } 158 | _final: 159 | pthread_mutex_unlock( &_cond_locker ); 160 | 161 | return ans; 162 | #endif 163 | 164 | } 165 | protected: 166 | 167 | void release() 168 | { 169 | #ifdef _WIN32 170 | CloseHandle(_event); 171 | #else 172 | pthread_mutex_destroy(&_cond_locker); 173 | pthread_cond_destroy(&_cond_var); 174 | #endif 175 | } 176 | 177 | #ifdef _WIN32 178 | HANDLE _event; 179 | #else 180 | pthread_cond_t _cond_var; 181 | pthread_mutex_t _cond_locker; 182 | bool _is_signalled; 183 | bool _isAutoReset; 184 | #endif 185 | }; 186 | }} 187 | -------------------------------------------------------------------------------- /sdk/src/hal/locker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | namespace rp{ namespace hal{ 37 | 38 | class Locker 39 | { 40 | public: 41 | enum LOCK_STATUS 42 | { 43 | LOCK_OK = 1, 44 | LOCK_TIMEOUT = -1, 45 | LOCK_FAILED = 0 46 | }; 47 | 48 | Locker(){ 49 | #ifdef _WIN32 50 | _lock = NULL; 51 | #endif 52 | init(); 53 | } 54 | 55 | ~Locker() 56 | { 57 | release(); 58 | } 59 | 60 | Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF) 61 | { 62 | #ifdef _WIN32 63 | switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) 64 | { 65 | case WAIT_ABANDONED: 66 | return LOCK_FAILED; 67 | case WAIT_OBJECT_0: 68 | return LOCK_OK; 69 | case WAIT_TIMEOUT: 70 | return LOCK_TIMEOUT; 71 | } 72 | 73 | #else 74 | #ifdef _MACOS 75 | if (timeout !=0 ) { 76 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 77 | } 78 | #else 79 | if (timeout == 0xFFFFFFFF){ 80 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 81 | } 82 | #endif 83 | else if (timeout == 0) 84 | { 85 | if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK; 86 | } 87 | #ifndef _MACOS 88 | else 89 | { 90 | timespec wait_time; 91 | timeval now; 92 | gettimeofday(&now,NULL); 93 | 94 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 95 | wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; 96 | 97 | if (wait_time.tv_nsec >= 1000000000) 98 | { 99 | ++wait_time.tv_sec; 100 | wait_time.tv_nsec -= 1000000000; 101 | } 102 | switch (pthread_mutex_timedlock(&_lock,&wait_time)) 103 | { 104 | case 0: 105 | return LOCK_OK; 106 | case ETIMEDOUT: 107 | return LOCK_TIMEOUT; 108 | } 109 | } 110 | #endif 111 | #endif 112 | 113 | return LOCK_FAILED; 114 | } 115 | 116 | 117 | void unlock() 118 | { 119 | #ifdef _WIN32 120 | ReleaseMutex(_lock); 121 | #else 122 | pthread_mutex_unlock(&_lock); 123 | #endif 124 | } 125 | 126 | #ifdef _WIN32 127 | HANDLE getLockHandle() 128 | { 129 | return _lock; 130 | } 131 | #else 132 | pthread_mutex_t *getLockHandle() 133 | { 134 | return &_lock; 135 | } 136 | #endif 137 | 138 | 139 | 140 | protected: 141 | void init() 142 | { 143 | #ifdef _WIN32 144 | _lock = CreateMutex(NULL,FALSE,NULL); 145 | #else 146 | pthread_mutex_init(&_lock, NULL); 147 | #endif 148 | } 149 | 150 | void release() 151 | { 152 | unlock(); //force unlock before release 153 | #ifdef _WIN32 154 | 155 | if (_lock) CloseHandle(_lock); 156 | _lock = NULL; 157 | #else 158 | pthread_mutex_destroy(&_lock); 159 | #endif 160 | } 161 | 162 | #ifdef _WIN32 163 | HANDLE _lock; 164 | #else 165 | pthread_mutex_t _lock; 166 | #endif 167 | }; 168 | 169 | class AutoLocker 170 | { 171 | public : 172 | AutoLocker(Locker &l): _binded(l) 173 | { 174 | _binded.lock(); 175 | } 176 | 177 | void forceUnlock() { 178 | _binded.unlock(); 179 | } 180 | ~AutoLocker() {_binded.unlock();} 181 | Locker & _binded; 182 | }; 183 | 184 | 185 | }} 186 | 187 | -------------------------------------------------------------------------------- /sdk/src/hal/socket.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RoboPeak Project 3 | * HAL Layer - Socket Interface 4 | * Copyright 2009 - 2013 RoboPeak Project 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | namespace rp{ namespace net { 12 | 13 | class _single_thread SocketAddress 14 | { 15 | 16 | public: 17 | enum address_type_t { 18 | ADDRESS_TYPE_UNSPEC = 0, 19 | ADDRESS_TYPE_INET = 1, 20 | ADDRESS_TYPE_INET6 = 2, 21 | }; 22 | 23 | public: 24 | 25 | 26 | 27 | SocketAddress(); 28 | SocketAddress(const char * addrString, int port, address_type_t = ADDRESS_TYPE_INET); 29 | // do not use this function, internal usage 30 | SocketAddress(void * platform_data); 31 | SocketAddress(const SocketAddress &); 32 | 33 | SocketAddress & operator = (const SocketAddress &); 34 | 35 | virtual ~SocketAddress(); 36 | 37 | virtual int getPort() const; 38 | virtual u_result setPort(int port); 39 | 40 | virtual u_result setAddressFromString(const char * address_string, address_type_t = ADDRESS_TYPE_INET); 41 | virtual u_result getAddressAsString(char * buffer, size_t buffersize) const; 42 | 43 | virtual address_type_t getAddressType() const; 44 | 45 | virtual u_result getRawAddress(_u8 * buffer, size_t bufferSize) const; 46 | 47 | const void * getPlatformData() const { 48 | return _platform_data; 49 | } 50 | 51 | virtual void setLoopbackAddress(address_type_t = ADDRESS_TYPE_INET); 52 | virtual void setBroadcastAddressIPv4(); 53 | virtual void setAnyAddress(address_type_t = ADDRESS_TYPE_INET); 54 | 55 | public: 56 | static size_t LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS = true, address_type_t = ADDRESS_TYPE_INET); 57 | 58 | protected: 59 | void * _platform_data; 60 | }; 61 | 62 | 63 | 64 | class SocketBase 65 | { 66 | public: 67 | enum socket_family_t { 68 | SOCKET_FAMILY_INET = 0, 69 | SOCKET_FAMILY_INET6 = 1, 70 | SOCKET_FAMILY_RAW = 2, 71 | }; 72 | 73 | 74 | enum socket_direction_mask { 75 | SOCKET_DIR_RD = 0x1, 76 | SOCKET_DIR_WR = 0x2, 77 | SOCKET_DIR_BOTH = (SOCKET_DIR_RD | SOCKET_DIR_WR), 78 | }; 79 | 80 | enum { 81 | DEFAULT_SOCKET_TIMEOUT = 10000, //10sec 82 | }; 83 | 84 | virtual ~SocketBase() {} 85 | virtual void dispose() = 0; 86 | virtual u_result bind(const SocketAddress & ) = 0; 87 | 88 | virtual u_result getLocalAddress(SocketAddress & ) = 0; 89 | virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk = SOCKET_DIR_BOTH) = 0; 90 | 91 | virtual u_result waitforSent(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; 92 | virtual u_result waitforData(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; 93 | protected: 94 | SocketBase() {} 95 | }; 96 | 97 | 98 | class _single_thread StreamSocket : public SocketBase 99 | { 100 | public: 101 | 102 | enum { 103 | MAX_BACKLOG = 128, 104 | }; 105 | 106 | static StreamSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); 107 | 108 | virtual u_result connect(const SocketAddress & pairAddress) = 0; 109 | 110 | virtual u_result listen(int backlog = MAX_BACKLOG) = 0; 111 | virtual StreamSocket * accept(SocketAddress * pairAddress = NULL) = 0; 112 | virtual u_result waitforIncomingConnection(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; 113 | 114 | virtual u_result send(const void * buffer, size_t len) = 0; 115 | 116 | virtual u_result recv(void *buf, size_t len, size_t & recv_len) = 0; 117 | 118 | virtual u_result getPeerAddress(SocketAddress & ) = 0; 119 | 120 | virtual u_result shutdown(socket_direction_mask mask) = 0; 121 | 122 | virtual u_result enableKeepAlive(bool enable = true) = 0; 123 | 124 | virtual u_result enableNoDelay(bool enable = true) = 0; 125 | 126 | protected: 127 | virtual ~StreamSocket() {} // use dispose(); 128 | StreamSocket() {} 129 | }; 130 | 131 | class _single_thread DGramSocket: public SocketBase 132 | { 133 | 134 | public: 135 | 136 | static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); 137 | 138 | 139 | 140 | virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) = 0; 141 | 142 | virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr = NULL) = 0; 143 | 144 | 145 | protected: 146 | virtual ~DGramSocket() {} // use dispose(); 147 | 148 | DGramSocket() {} 149 | }; 150 | 151 | }} 152 | -------------------------------------------------------------------------------- /sdk/src/hal/thread.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #include "sdkcommon.h" 36 | #include "hal/thread.h" 37 | 38 | #if defined(_WIN32) 39 | #include "arch/win32/winthread.hpp" 40 | #elif defined(_MACOS) 41 | #include "arch/macOS/thread.hpp" 42 | #elif defined(__GNUC__) 43 | #include "arch/linux/thread.hpp" 44 | #else 45 | #error no threading implemention found for this platform. 46 | #endif 47 | 48 | 49 | -------------------------------------------------------------------------------- /sdk/src/hal/thread.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal/types.h" 38 | #define CLASS_THREAD(c , x ) \ 39 | rp::hal::Thread::create_member(this ) 40 | 41 | namespace rp{ namespace hal{ 42 | 43 | class Thread 44 | { 45 | public: 46 | enum priority_val_t 47 | { 48 | PRIORITY_REALTIME = 0, 49 | PRIORITY_HIGH = 1, 50 | PRIORITY_NORMAL = 2, 51 | PRIORITY_LOW = 3, 52 | PRIORITY_IDLE = 4, 53 | }; 54 | 55 | template 56 | static Thread create_member(T * pthis) 57 | { 58 | return create(_thread_thunk, pthis); 59 | } 60 | 61 | template 62 | static _word_size_t THREAD_PROC _thread_thunk(void * data) 63 | { 64 | return (static_cast(data)->*PROC)(); 65 | } 66 | static Thread create(thread_proc_t proc, void * data = NULL ); 67 | 68 | public: 69 | ~Thread() { } 70 | Thread(): _data(NULL),_func(NULL),_handle(0) {} 71 | _word_size_t getHandle(){ return _handle;} 72 | u_result terminate(); 73 | void *getData() { return _data;} 74 | u_result join(unsigned long timeout = -1); 75 | u_result setPriority( priority_val_t p); 76 | priority_val_t getPriority(); 77 | 78 | bool operator== ( const Thread & right) { return this->_handle == right._handle; } 79 | protected: 80 | Thread( thread_proc_t proc, void * data ): _data(data),_func(proc), _handle(0) {} 81 | void * _data; 82 | thread_proc_t _func; 83 | _word_size_t _handle; 84 | }; 85 | 86 | }} 87 | 88 | -------------------------------------------------------------------------------- /sdk/src/hal/types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Common Data Types for RP 3 | */ 4 | 5 | #ifndef _INFRA_HAL_TYPES_H_ 6 | #define _INFRA_HAL_TYPES_H_ 7 | 8 | //Basic types 9 | // 10 | #ifdef WIN32 11 | 12 | //fake stdint.h for VC only 13 | 14 | typedef signed char int8_t; 15 | typedef unsigned char uint8_t; 16 | 17 | typedef __int16 int16_t; 18 | typedef unsigned __int16 uint16_t; 19 | 20 | typedef __int32 int32_t; 21 | typedef unsigned __int32 uint32_t; 22 | 23 | typedef __int64 int64_t; 24 | typedef unsigned __int64 uint64_t; 25 | 26 | 27 | #define RPMODULE_EXPORT __declspec(dllexport) 28 | #define RPMODULE_IMPORT __declspec(dllimport) 29 | 30 | #else 31 | 32 | #include 33 | 34 | #define RPMODULE_EXPORT 35 | #define RPMODULE_IMPORT 36 | 37 | #endif 38 | 39 | 40 | //based on stdint.h 41 | typedef int8_t _s8; 42 | typedef uint8_t _u8; 43 | 44 | typedef int16_t _s16; 45 | typedef uint16_t _u16; 46 | 47 | typedef int32_t _s32; 48 | typedef uint32_t _u32; 49 | 50 | typedef int64_t _s64; 51 | typedef uint64_t _u64; 52 | 53 | #define __small_endian 54 | 55 | #ifndef __GNUC__ 56 | #define __attribute__(x) 57 | #endif 58 | 59 | 60 | // The _word_size_t uses actual data bus width of the current CPU 61 | #ifdef _AVR_ 62 | typedef _u8 _word_size_t; 63 | #define THREAD_PROC 64 | #elif defined (WIN64) 65 | typedef _u64 _word_size_t; 66 | #define THREAD_PROC __stdcall 67 | #elif defined (WIN32) 68 | typedef _u32 _word_size_t; 69 | #define THREAD_PROC __stdcall 70 | #elif defined (__GNUC__) 71 | typedef unsigned long _word_size_t; 72 | #define THREAD_PROC 73 | #elif defined (__ICCARM__) 74 | typedef _u32 _word_size_t; 75 | #define THREAD_PROC 76 | #endif 77 | 78 | 79 | 80 | #define __le 81 | #define __be 82 | 83 | #define _multi_thread 84 | #define _single_thread 85 | 86 | typedef uint32_t u_result; 87 | 88 | #define RESULT_OK 0 89 | #define RESULT_FAIL_BIT 0x80000000 90 | #define RESULT_ALREADY_DONE 0x20 91 | #define RESULT_REMAINING_DATA 0x21 92 | #define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) 93 | #define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) 94 | #define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) 95 | #define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) 96 | #define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) 97 | #define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) 98 | #define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) 99 | #define RESULT_OPERATION_ABORTED (0x8007 | RESULT_FAIL_BIT) 100 | #define RESULT_NOT_FOUND (0x8008 | RESULT_FAIL_BIT) 101 | #define RESULT_RECONNECTING (0x8009 | RESULT_FAIL_BIT) 102 | 103 | #define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) 104 | #define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) 105 | 106 | 107 | typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); 108 | 109 | 110 | #if defined (_BUILD_AS_DLL) 111 | #if defined (_BUILD_DLL_EXPORT) 112 | #define RPMODULE_IMPEXP RPMODULE_EXPORT 113 | #else 114 | #define RPMODULE_IMPEXP RPMODULE_IMPORT 115 | #endif 116 | #else 117 | #define RPMODULE_IMPEXP 118 | #endif 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /sdk/src/hal/util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | 38 | //------ 39 | /* _countof helper */ 40 | #if !defined(_countof) 41 | #if !defined(__cplusplus) 42 | #define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0])) 43 | #else 44 | extern "C++" 45 | { 46 | template 47 | char (*__countof_helper( _CountofType (&_Array)[_SizeOfArray]))[_SizeOfArray]; 48 | #define _countof(_Array) sizeof(*__countof_helper(_Array)) 49 | } 50 | #endif 51 | #endif 52 | 53 | /* _offsetof helper */ 54 | #if !defined(offsetof) 55 | #define offsetof(_structure, _field) ((_word_size_t)&(((_structure *)0x0)->_field)) 56 | #endif 57 | 58 | 59 | #define BEGIN_STATIC_CODE( _blockname_ ) \ 60 | static class _static_code_##_blockname_ { \ 61 | public: \ 62 | _static_code_##_blockname_ () 63 | 64 | 65 | #define END_STATIC_CODE( _blockname_ ) \ 66 | } _instance_##_blockname_; 67 | 68 | -------------------------------------------------------------------------------- /sdk/src/rplidar_driver_TCP.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | namespace rp { namespace standalone{ namespace rplidar { 38 | 39 | class TCPChannelDevice :public ChannelDevice 40 | { 41 | public: 42 | rp::net::StreamSocket * _binded_socket; 43 | TCPChannelDevice():_binded_socket(rp::net::StreamSocket::CreateSocket()){} 44 | 45 | bool bind(const char * ipStr, uint32_t port) 46 | { 47 | rp::net::SocketAddress socket(ipStr, port); 48 | return IS_OK(_binded_socket->connect(socket)); 49 | } 50 | void close() 51 | { 52 | _binded_socket->dispose(); 53 | _binded_socket = NULL; 54 | } 55 | bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) 56 | { 57 | if(returned_size) 58 | *returned_size = data_count; 59 | return (_binded_socket->waitforData(timeout) == RESULT_OK); 60 | } 61 | int senddata(const _u8 * data, size_t size) 62 | { 63 | return _binded_socket->send(data, size) ; 64 | } 65 | int recvdata(unsigned char * data, size_t size) 66 | { 67 | size_t lenRec = 0; 68 | _binded_socket->recv(data, size, lenRec); 69 | return lenRec; 70 | } 71 | }; 72 | 73 | 74 | class RPlidarDriverTCP : public RPlidarDriverImplCommon 75 | { 76 | public: 77 | 78 | RPlidarDriverTCP(); 79 | virtual ~RPlidarDriverTCP(); 80 | virtual u_result connect(const char * ipStr, _u32 port, _u32 flag = 0); 81 | virtual void disconnect(); 82 | }; 83 | 84 | 85 | }}} -------------------------------------------------------------------------------- /sdk/src/rplidar_driver_impl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | namespace rp { namespace standalone{ namespace rplidar { 38 | class RPlidarDriverImplCommon : public RPlidarDriver 39 | { 40 | public: 41 | enum { 42 | RPLIDAR_TOF_MINUM_MAJOR_ID = 5, 43 | }; 44 | 45 | virtual bool isConnected(); 46 | virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT); 47 | virtual u_result clearNetSerialRxCache(); 48 | virtual u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); 49 | virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); 50 | virtual u_result checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs = DEFAULT_TIMEOUT); 51 | 52 | virtual u_result getScanModeCount(_u16& modeCount, _u32 timeoutInMs = DEFAULT_TIMEOUT); 53 | virtual u_result getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); 54 | virtual u_result getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); 55 | virtual u_result getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); 56 | virtual u_result getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); 57 | virtual u_result getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve = std::vector<_u8>(), _u32 timeout = DEFAULT_TIMEOUT); 58 | 59 | virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL); 60 | virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT); 61 | 62 | 63 | virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); 64 | virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); 65 | virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT); 66 | virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT); 67 | virtual u_result setMotorPWM(_u16 pwm); 68 | virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT); 69 | virtual u_result startMotor(); 70 | virtual u_result stopMotor(); 71 | virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); 72 | virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode); 73 | virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency); 74 | virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT); 75 | virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT); 76 | virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT); 77 | virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); 78 | virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); 79 | virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count); 80 | virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count); 81 | virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count); 82 | virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count); 83 | 84 | protected: 85 | 86 | virtual u_result _sendCommand(_u8 cmd, const void * payload = NULL, size_t payloadsize = 0); 87 | void _disableDataGrabbing(); 88 | 89 | virtual u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout = DEFAULT_TIMEOUT); 90 | virtual u_result _cacheScanData(); 91 | virtual u_result _waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); 92 | virtual u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout = DEFAULT_TIMEOUT); 93 | virtual u_result _cacheCapsuledScanData(); 94 | virtual u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); 95 | virtual int _getSyncBitByAngle(const int current_angle_q16, const int angleInc_q16); 96 | virtual void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); 97 | virtual void _dense_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); 98 | 99 | //FW1.23 100 | virtual u_result _cacheUltraCapsuledScanData(); 101 | virtual u_result _waitUltraCapsuledNode(rplidar_response_ultra_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); 102 | virtual void _ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); 103 | 104 | virtual u_result _cacheHqScanData(); 105 | virtual u_result _waitHqNode(rplidar_response_hq_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); 106 | virtual void _HqToNormal(const rplidar_response_hq_capsule_measurement_nodes_t & node_hq, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); 107 | 108 | bool _isConnected; 109 | bool _isScanning; 110 | bool _isSupportingMotorCtrl; 111 | bool _isTofLidar; 112 | rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; 113 | size_t _cached_scan_node_hq_count; 114 | 115 | rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192]; 116 | size_t _cached_scan_node_hq_count_for_interval_retrieve; 117 | 118 | _u16 _cached_sampleduration_std; 119 | _u16 _cached_sampleduration_express; 120 | _u8 _cached_express_flag; 121 | float _cached_current_us_per_sample; 122 | 123 | rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; 124 | rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; 125 | rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; 126 | rplidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; 127 | bool _is_previous_capsuledataRdy; 128 | bool _is_previous_HqdataRdy; 129 | bool _syncBit_is_finded; 130 | 131 | 132 | 133 | rp::hal::Locker _lock; 134 | rp::hal::Event _dataEvt; 135 | rp::hal::Thread _cachethread; 136 | 137 | protected: 138 | RPlidarDriverImplCommon(); 139 | virtual ~RPlidarDriverImplCommon() {} 140 | }; 141 | }}} -------------------------------------------------------------------------------- /sdk/src/rplidar_driver_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | namespace rp { namespace standalone{ namespace rplidar { 38 | 39 | class SerialChannelDevice :public ChannelDevice 40 | { 41 | public: 42 | rp::hal::serial_rxtx * _rxtxSerial; 43 | bool _closePending; 44 | 45 | SerialChannelDevice():_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()){} 46 | 47 | bool bind(const char * portname, uint32_t baudrate) 48 | { 49 | _closePending = false; 50 | return _rxtxSerial->bind(portname, baudrate); 51 | } 52 | bool open() 53 | { 54 | return _rxtxSerial->open(); 55 | } 56 | void close() 57 | { 58 | _closePending = true; 59 | _rxtxSerial->cancelOperation(); 60 | _rxtxSerial->close(); 61 | } 62 | void flush() 63 | { 64 | _rxtxSerial->flush(0); 65 | } 66 | bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) 67 | { 68 | if (_closePending) return false; 69 | return (_rxtxSerial->waitfordata(data_count, timeout, returned_size) == rp::hal::serial_rxtx::ANS_OK); 70 | } 71 | int senddata(const _u8 * data, size_t size) 72 | { 73 | return _rxtxSerial->senddata(data, size) ; 74 | } 75 | int recvdata(unsigned char * data, size_t size) 76 | { 77 | size_t lenRec = 0; 78 | lenRec = _rxtxSerial->recvdata(data, size); 79 | return lenRec; 80 | } 81 | void setDTR() 82 | { 83 | _rxtxSerial->setDTR(); 84 | } 85 | void clearDTR() 86 | { 87 | _rxtxSerial->clearDTR(); 88 | } 89 | void ReleaseRxTx() 90 | { 91 | rp::hal::serial_rxtx::ReleaseRxTx(_rxtxSerial); 92 | } 93 | }; 94 | 95 | class RPlidarDriverSerial : public RPlidarDriverImplCommon 96 | { 97 | public: 98 | 99 | RPlidarDriverSerial(); 100 | virtual ~RPlidarDriverSerial(); 101 | virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag = 0); 102 | virtual void disconnect(); 103 | 104 | }; 105 | 106 | }}} 107 | -------------------------------------------------------------------------------- /sdk/src/sdkcommon.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RPLIDAR SDK 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | */ 10 | /* 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 25 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 28 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 30 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 31 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | #if defined(_WIN32) 36 | #include "arch\win32\arch_win32.h" 37 | #elif defined(_MACOS) 38 | #include "arch/macOS/arch_macOS.h" 39 | #elif defined(__GNUC__) 40 | #include "arch/linux/arch_linux.h" 41 | #else 42 | #error "unsupported target" 43 | #endif 44 | 45 | #include "hal/types.h" 46 | #include "hal/assert.h" 47 | 48 | #include "rplidar.h" 49 | 50 | #include "hal/util.h" -------------------------------------------------------------------------------- /src/rplidar_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Rplidar ROS NODE 3 | * 4 | * Copyright (c) 2009 - 2014 RoboPeak Team 5 | * http://www.robopeak.com 6 | * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. 7 | * http://www.slamtec.com 8 | * 9 | * Copyright (c) 2019 Hunter L. Allen 10 | */ 11 | /* 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * 1. Redistributions of source code must retain the above copyright notice, 16 | * this list of conditions and the following disclaimer. 17 | * 18 | * 2. Redistributions in binary form must reproduce the above copyright notice, 19 | * this list of conditions and the following disclaimer in the documentation 20 | * and/or other materials provided with the distribution. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 24 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 25 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 26 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 27 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 28 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 29 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 30 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 31 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 32 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | */ 35 | 36 | #include 37 | 38 | namespace rplidar_ros 39 | { 40 | 41 | rplidar_node::rplidar_node(const rclcpp::NodeOptions & options) 42 | : rclcpp::Node("rplidar_node", options) 43 | { 44 | /* set parameters */ 45 | channel_type_ = this->declare_parameter("channel_type", "serial"); 46 | tcp_ip_ = this->declare_parameter("tcp_ip", "192.168.0.7"); 47 | tcp_port_ = this->declare_parameter("tcp_port", 20108); 48 | serial_port_ = this->declare_parameter("serial_port", "/dev/ttyUSB0"); 49 | serial_baudrate_ = this->declare_parameter("serial_baudrate", 115200); 50 | frame_id_ = this->declare_parameter("frame_id", std::string("laser_frame")); 51 | inverted_ = this->declare_parameter("inverted", false); 52 | angle_compensate_ = this->declare_parameter("angle_compensate", false); 53 | flip_x_axis_ = this->declare_parameter("flip_x_axis", false); 54 | scan_mode_ = this->declare_parameter("scan_mode", std::string()); 55 | topic_name_ = this->declare_parameter("topic_name", std::string("scan")); 56 | 57 | RCLCPP_INFO( 58 | this->get_logger(), 59 | "RPLIDAR running on ROS 2 package rplidar_ros. SDK Version: '%s'", RPLIDAR_SDK_VERSION); 60 | 61 | /* initialize SDK */ 62 | m_drv = (channel_type_ == "tcp") ? 63 | RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP) : 64 | RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT); 65 | 66 | if (nullptr == m_drv) { 67 | /* don't start spinning without a driver object */ 68 | RCLCPP_ERROR(this->get_logger(), "Failed to construct driver"); 69 | return; 70 | } 71 | 72 | if (channel_type_ == "tcp") { 73 | // make connection... 74 | if (IS_FAIL(m_drv->connect(tcp_ip_.c_str(), (_u32)tcp_port_))) { 75 | RCLCPP_ERROR( 76 | this->get_logger(), 77 | "Error, cannot bind to the specified TCP host '%s:%ud'", 78 | tcp_ip_.c_str(), static_cast(tcp_port_)); 79 | RPlidarDriver::DisposeDriver(m_drv); 80 | return; 81 | } 82 | } else { 83 | // make connection... 84 | if (IS_FAIL(m_drv->connect(serial_port_.c_str(), (_u32)serial_baudrate_))) { 85 | RCLCPP_ERROR( 86 | this->get_logger(), "Error, cannot bind to the specified serial port '%s'.", 87 | serial_port_.c_str()); 88 | RPlidarDriver::DisposeDriver(m_drv); 89 | return; 90 | } 91 | } 92 | 93 | // get rplidar device info 94 | if (!getRPLIDARDeviceInfo()) { 95 | /* don't continue */ 96 | RPlidarDriver::DisposeDriver(m_drv); 97 | return; 98 | } 99 | 100 | // check health... 101 | if (!checkRPLIDARHealth()) { 102 | RPlidarDriver::DisposeDriver(m_drv); 103 | return; 104 | } 105 | 106 | /* start motor */ 107 | m_drv->startMotor(); 108 | 109 | if (!set_scan_mode()) { 110 | /* set the scan mode */ 111 | m_drv->stop(); 112 | m_drv->stopMotor(); 113 | RPlidarDriver::DisposeDriver(m_drv); 114 | exit(1); 115 | } 116 | 117 | /* done setting up RPLIDAR stuff, now set up ROS 2 stuff */ 118 | 119 | /* create the publisher for "/scan" */ 120 | m_publisher = this->create_publisher(topic_name_, 10); 121 | 122 | /* create stop motor service */ 123 | m_stop_motor_service = this->create_service( 124 | "stop_motor", 125 | std::bind(&rplidar_node::stop_motor, this, std::placeholders::_1, std::placeholders::_2)); 126 | 127 | /* create start motor service */ 128 | m_start_motor_service = this->create_service( 129 | "start_motor", 130 | std::bind(&rplidar_node::start_motor, this, std::placeholders::_1, std::placeholders::_2)); 131 | /* start timer */ 132 | m_timer = this->create_wall_timer(1ms, std::bind(&rplidar_node::publish_loop, this)); 133 | } 134 | 135 | rplidar_node::~rplidar_node() 136 | { 137 | m_drv->stop(); 138 | m_drv->stopMotor(); 139 | RPlidarDriver::DisposeDriver(m_drv); 140 | } 141 | 142 | void rplidar_node::publish_scan( 143 | const double scan_time, ResponseNodeArray nodes, size_t node_count) 144 | { 145 | static size_t scan_count = 0; 146 | sensor_msgs::msg::LaserScan scan_msg; 147 | 148 | /* NOTE(allenh1): time was passed in as a parameter before */ 149 | scan_msg.header.stamp = this->now(); 150 | scan_msg.header.frame_id = frame_id_; 151 | scan_count++; 152 | 153 | bool reversed = (angle_max > angle_min); 154 | if (reversed) { 155 | /* NOTE(allenh1): the other case seems impossible? */ 156 | scan_msg.angle_min = M_PI - angle_max; 157 | scan_msg.angle_max = M_PI - angle_min; 158 | } else { 159 | scan_msg.angle_min = M_PI - angle_min; 160 | scan_msg.angle_max = M_PI - angle_max; 161 | } 162 | scan_msg.angle_increment = 163 | (scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count - 1); 164 | 165 | scan_msg.scan_time = scan_time; 166 | scan_msg.time_increment = scan_time / (double)(node_count - 1); 167 | scan_msg.range_min = min_distance; 168 | scan_msg.range_max = max_distance; 169 | 170 | scan_msg.intensities.resize(node_count); 171 | scan_msg.ranges.resize(node_count); 172 | bool reverse_data = (!inverted_ && reversed) || (inverted_ && !reversed); 173 | size_t scan_midpoint = node_count / 2; 174 | for (size_t i = 0; i < node_count; ++i) { 175 | float read_value = (float) nodes[i].dist_mm_q2 / 4.0f / 1000; 176 | size_t apply_index = i; 177 | if (reverse_data) { 178 | apply_index = node_count - 1 - i; 179 | } 180 | if (flip_x_axis_) { 181 | if (apply_index >= scan_midpoint) { 182 | apply_index = apply_index - scan_midpoint; 183 | } else { 184 | apply_index = apply_index + scan_midpoint; 185 | } 186 | } 187 | if (read_value == 0.0) { 188 | scan_msg.ranges[apply_index] = std::numeric_limits::infinity(); 189 | } else { 190 | scan_msg.ranges[apply_index] = read_value; 191 | } 192 | scan_msg.intensities[apply_index] = (float) (nodes[i].quality >> 2); 193 | } 194 | 195 | m_publisher->publish(scan_msg); 196 | } 197 | 198 | 199 | bool rplidar_node::getRPLIDARDeviceInfo() const 200 | { 201 | u_result op_result; 202 | rplidar_response_device_info_t devinfo; 203 | 204 | op_result = m_drv->getDeviceInfo(devinfo); 205 | if (IS_FAIL(op_result)) { 206 | if (op_result == RESULT_OPERATION_TIMEOUT) { 207 | RCLCPP_ERROR(this->get_logger(), "Error, operation time out. RESULT_OPERATION_TIMEOUT!"); 208 | } else { 209 | RCLCPP_ERROR(this->get_logger(), "Error, unexpected error, code: '%x'", op_result); 210 | } 211 | return false; 212 | } 213 | 214 | // print out the device serial number, firmware and hardware version number.. 215 | std::string serial_no{"RPLIDAR S/N: "}; 216 | for (int pos = 0; pos < 16; ++pos) { 217 | char buff[3]; 218 | snprintf(buff, sizeof(buff), "%02X", devinfo.serialnum[pos]); 219 | serial_no += buff; 220 | } 221 | RCLCPP_INFO(this->get_logger(), "%s", serial_no.c_str()); 222 | RCLCPP_INFO( 223 | this->get_logger(), "Firmware Ver: %d.%02d", devinfo.firmware_version >> 8, 224 | devinfo.firmware_version & 0xFF); 225 | RCLCPP_INFO(this->get_logger(), "Hardware Rev: %d", static_cast(devinfo.hardware_version)); 226 | return true; 227 | } 228 | 229 | bool rplidar_node::checkRPLIDARHealth() const 230 | { 231 | rplidar_response_device_health_t healthinfo; 232 | u_result op_result = m_drv->getHealth(healthinfo); 233 | 234 | if (IS_OK(op_result)) { 235 | RCLCPP_INFO(this->get_logger(), "RPLidar health status : '%d'", healthinfo.status); 236 | if (healthinfo.status == RPLIDAR_STATUS_ERROR) { 237 | RCLCPP_ERROR( 238 | this->get_logger(), 239 | "Error, rplidar internal error detected. Please reboot the device to retry"); 240 | return false; 241 | } 242 | return true; 243 | } 244 | RCLCPP_ERROR(this->get_logger(), "Error, cannot retrieve rplidar health code: '%x'", op_result); 245 | return false; 246 | } 247 | 248 | void rplidar_node::stop_motor(const EmptyRequest req, EmptyResponse res) 249 | { 250 | if (nullptr == m_drv) { 251 | return; 252 | } 253 | 254 | RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__); 255 | m_drv->stop(); 256 | m_drv->stopMotor(); 257 | } 258 | 259 | void rplidar_node::start_motor(const EmptyRequest req, EmptyResponse res) 260 | { 261 | if (nullptr == m_drv) { 262 | return; 263 | } 264 | 265 | RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__); 266 | m_drv->startMotor(); 267 | m_drv->startScan(0, 1); 268 | } 269 | 270 | bool rplidar_node::set_scan_mode() 271 | { 272 | u_result op_result; 273 | RplidarScanMode current_scan_mode; 274 | if (scan_mode_.empty()) { 275 | op_result = m_drv->startScan( 276 | false /* not force scan */, true /* use typical scan mode */, 0, 277 | ¤t_scan_mode); 278 | } else { 279 | std::vector allSupportedScanModes; 280 | op_result = m_drv->getAllSupportedScanModes(allSupportedScanModes); 281 | if (IS_OK(op_result)) { 282 | auto iter = std::find_if( 283 | allSupportedScanModes.begin(), allSupportedScanModes.end(), 284 | [this](auto s1) { 285 | return std::string(s1.scan_mode) == scan_mode_; 286 | }); 287 | if (iter == allSupportedScanModes.end()) { 288 | RCLCPP_ERROR( 289 | this->get_logger(), "scan mode `%s' is not supported by lidar, supported modes ('%zd'):", 290 | scan_mode_.c_str(), allSupportedScanModes.size()); 291 | for (const auto & it : allSupportedScanModes) { 292 | RCLCPP_ERROR( 293 | this->get_logger(), "%s: max_distance: %.1f m, Point number: %.1fK", 294 | it.scan_mode, it.max_distance, (1000 / it.us_per_sample)); 295 | } 296 | op_result = RESULT_OPERATION_FAIL; 297 | return false; 298 | } else { 299 | op_result = m_drv->startScanExpress( 300 | false /* not force scan */, iter->id, 0, 301 | ¤t_scan_mode); 302 | } 303 | } 304 | } 305 | 306 | /* verify we set the scan mode */ 307 | if (!IS_OK(op_result)) { 308 | RCLCPP_ERROR(this->get_logger(), "Cannot start scan: '%08x'", op_result); 309 | return false; 310 | } 311 | 312 | // default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us 313 | m_angle_compensate_multiple = 314 | static_cast(1000 * 1000 / current_scan_mode.us_per_sample / 10.0 / 360.0); 315 | if (m_angle_compensate_multiple < 1) { 316 | m_angle_compensate_multiple = 1; 317 | } 318 | max_distance = current_scan_mode.max_distance; 319 | RCLCPP_INFO( 320 | this->get_logger(), 321 | "current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d, flip_x_axis %d", current_scan_mode.scan_mode, 322 | current_scan_mode.max_distance, (1000 / current_scan_mode.us_per_sample), 323 | m_angle_compensate_multiple, flip_x_axis_); 324 | return true; 325 | } 326 | 327 | void rplidar_node::publish_loop() 328 | { 329 | rclcpp::Time start_scan_time; 330 | rclcpp::Time end_scan_time; 331 | u_result op_result; 332 | size_t count = 360 * 8; 333 | auto nodes = std::make_unique(count); 334 | 335 | start_scan_time = this->now(); 336 | op_result = m_drv->grabScanDataHq(nodes.get(), count); 337 | end_scan_time = this->now(); 338 | double scan_duration = (end_scan_time - start_scan_time).nanoseconds() * 1E-9; 339 | 340 | if (op_result != RESULT_OK) { 341 | return; 342 | } 343 | op_result = m_drv->ascendScanData(nodes.get(), count); 344 | angle_min = deg_2_rad(0.0f); 345 | angle_max = deg_2_rad(359.0f); 346 | if (op_result == RESULT_OK) { 347 | if (angle_compensate_) { 348 | const int angle_compensate_nodes_count = 360 * m_angle_compensate_multiple; 349 | int angle_compensate_offset = 0; 350 | auto angle_compensate_nodes = std::make_unique( 351 | angle_compensate_nodes_count); 352 | memset( 353 | angle_compensate_nodes.get(), 0, 354 | angle_compensate_nodes_count * sizeof(rplidar_response_measurement_node_hq_t)); 355 | 356 | size_t i = 0, j = 0; 357 | for (; i < count; i++) { 358 | if (nodes[i].dist_mm_q2 != 0) { 359 | float angle = getAngle(nodes[i]); 360 | int angle_value = (int)(angle * m_angle_compensate_multiple); 361 | if ((angle_value - angle_compensate_offset) < 0) {angle_compensate_offset = angle_value;} 362 | for (j = 0; j < m_angle_compensate_multiple; j++) { 363 | angle_compensate_nodes[angle_value - angle_compensate_offset + j] = nodes[i]; 364 | } 365 | } 366 | } 367 | 368 | publish_scan(scan_duration, std::move(angle_compensate_nodes), angle_compensate_nodes_count); 369 | } else { 370 | int start_node = 0, end_node = 0; 371 | int i = 0; 372 | // find the first valid node and last valid node 373 | while (nodes[i++].dist_mm_q2 == 0) {} 374 | start_node = i - 1; 375 | i = count - 1; 376 | while (nodes[i--].dist_mm_q2 == 0) {} 377 | end_node = i + 1; 378 | 379 | angle_min = deg_2_rad(getAngle(nodes[start_node])); 380 | angle_max = deg_2_rad(getAngle(nodes[end_node])); 381 | auto valid = std::make_unique( 382 | end_node - start_node + 1); 383 | for (size_t x = start_node, y = 0; x < end_node; ++x, ++y) { 384 | valid[y] = nodes[x]; 385 | } 386 | publish_scan(scan_duration, std::move(valid), end_node - start_node + 1); 387 | } 388 | } else if (op_result == RESULT_OPERATION_FAIL) { 389 | // All the data is invalid, just publish them 390 | float angle_min = deg_2_rad(0.0f); 391 | float angle_max = deg_2_rad(359.0f); 392 | 393 | publish_scan(scan_duration, std::move(nodes), count); 394 | } 395 | } 396 | 397 | } // namespace rplidar_ros 398 | 399 | #include "rclcpp_components/register_node_macro.hpp" 400 | 401 | RCLCPP_COMPONENTS_REGISTER_NODE(rplidar_ros::rplidar_node) 402 | -------------------------------------------------------------------------------- /src/standalone_rplidar.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Hunter L. allen 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | rclcpp::init(argc, argv); 22 | rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); 23 | rclcpp::shutdown(); 24 | return 0; 25 | } 26 | --------------------------------------------------------------------------------