commit 457f054053fbd5277ca31ab40275a1d0802bbbc0 Author: Matt Spencer Date: Wed Apr 22 11:51:45 2026 +0000 Squashed 'lidar/sllidar_ros2/' content from commit 3430009 git-subtree-dir: lidar/sllidar_ros2 git-subtree-split: 34300099fadfc772965962dec837bf436706188f diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..aa094b1 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,81 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(sllidar_ros2) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(MSVC) + add_compile_definitions( + _USE_MATH_DEFINES + ) +endif() + +set(SLLIDAR_SDK_PATH "./sdk/") + +FILE(GLOB SLLIDAR_SDK_SRC + "${SLLIDAR_SDK_PATH}/src/arch/linux/*.cpp" + "${SLLIDAR_SDK_PATH}/src/hal/*.cpp" + "${SLLIDAR_SDK_PATH}/src/*.cpp" + "${SLLIDAR_SDK_PATH}/src/dataunpacker/*.cpp" + "${SLLIDAR_SDK_PATH}/src/dataunpacker/unpacker/*.cpp" +) + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +################################################################################ +# Build +################################################################################ +include_directories( + ${SLLIDAR_SDK_PATH}/include + ${SLLIDAR_SDK_PATH}/src + ${Boost_INCLUDE_DIRS} +) + +add_executable(sllidar_node src/sllidar_node.cpp ${SLLIDAR_SDK_SRC}) +# target_link_libraries(sllidar_node ${ament_cmake_LIBRARIES}) +ament_target_dependencies(sllidar_node + rclcpp + std_srvs + sensor_msgs +) + +add_executable(sllidar_client src/sllidar_client.cpp) +ament_target_dependencies(sllidar_client + rclcpp + std_srvs + sensor_msgs +) +################################################################################ +# Install +################################################################################ +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} +) + +install( + TARGETS sllidar_node sllidar_client + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +################################################################################ +# Macro for ament package +################################################################################ +ament_export_dependencies(rclcpp) +ament_export_dependencies(std_msgs) +ament_export_dependencies(sensor_msgs) +ament_export_include_directories(include) +ament_package() diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..144df83 --- /dev/null +++ b/LICENSE @@ -0,0 +1,24 @@ +Copyright (c) 2009 - 2014 RoboPeak Team +Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..ccb81d5 --- /dev/null +++ b/README.md @@ -0,0 +1,182 @@ +# SLAMTEC LIDAR ROS2 Package + +ROS2 node for SLAMTEC LIDAR + +Visit following Website for more details about SLAMTEC LIDAR: + +SLAMTEC LIDAR roswiki: + +SLAMTEC LIDAR HomePage: + +SLAMTEC LIDAR SDK: + +SLAMTEC LIDAR Tutorial: + +## Supported SLAMTEC LIDAR + +| Lidar Model | +| ---------------------- | +|RPLIDAR A1 | +|RPLIDAR A2 | +|RPLIDAR A3 | +|RPLIDAR C1 | +|RPLIDAR S1 | +|RPLIDAR S2 | +|RPLIDAR S3 | +|RPLIDAR S2E | +|RPLIDAR T1 | + +## How to install ROS2 + +[rolling](https://docs.ros.org/en/rolling/Installation.html), +[humble](https://docs.ros.org/en/humble/Installation.html), +[galactic](https://docs.ros.org/en/galactic/Installation.html), +[foxy](https://docs.ros.org/en/foxy/Installation.html) + +## How to configuring your ROS 2 environment + +[Configuring your ROS 2 environment](https://docs.ros.org/en/foxy/Tutorials/Configuring-ROS2-Environment.html) + +## How to Create a ROS2 workspace + +[ROS2 Tutorials Creating a workspace](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html) + +1. example, choose the directory name ros2_ws, for "development workspace" : + + ```bash + mkdir -p ~/ros2_ws/src + cd ~/ros2_ws/src + ``` + +## Compile & Install sllidar_ros2 package + +1. Clone sllidar_ros2 package from github + + Ensure you're still in the ros2_ws/src directory before you clone: + + ```bash + git clone https://github.com/Slamtec/sllidar_ros2.git + ``` + +2. Build sllidar_ros2 package + + From the root of your workspace (ros2_ws), you can now build sllidar_ros2 package using the command: + + ```bash + cd ~/ros2_ws/ + source /opt/ros//setup.bash + colcon build --symlink-install + ``` + + if you find output like "colcon:command not found",you need separate [install colcon](https://docs.ros.org/en/foxy/Tutorials/Colcon-Tutorial.html#install-colcon) build tools. + +3. Package environment setup + + ```bash + source ./install/setup.bash + ``` + + Note: Add permanent workspace environment variables. + It's convenientif the ROS2 environment variables are automatically added to your bash session every time a new shell is launched: + + ```bash + $echo "source /install/setup.bash" >> ~/.bashrc + $source ~/.bashrc + ``` + +4. Create udev rules for rplidar + + sllidar_ros2 running requires the read and write permissions of the serial device. + You can manually modify it with the following command: + + ```bash + sudo chmod 777 /dev/ttyUSB0 + ``` + + But a better way is to create a udev rule: + + ```bash + cd src/rpldiar_ros/ + source scripts/create_udev_rules.sh + ``` + +## Run sllidar_ros2 + +### Run sllidar node and view in the rviz + +The command for RPLIDAR A1 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_a1_launch.py +``` + +The command for RPLIDAR A2M7 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_a2m7_launch.py +``` + +The command for RPLIDAR A2M8 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_a2m8_launch.py +``` + +The command for RPLIDAR A2M12 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_a2m12_launch.py +``` + +The command for RPLIDAR A3 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_a3_launch.py +``` + +The command for RPLIDAR C1 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_c1_launch.py +``` + +The command for RPLIDAR S1 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_s1_launch.py +``` + +The command for RPLIDAR S2 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_s2_launch.py +``` + +```bash +ros2 launch sllidar_ros2 view_sllidar_s2e_launch.py +``` + +The command for RPLIDAR S3 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_s3_launch.py +``` + + +The command for RPLIDAR T1 is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_t1_launch.py +``` + +The command for RPLIDAR S1(TCP connection) is : + +```bash +ros2 launch sllidar_ros2 view_sllidar_s1_tcp_launch.py +``` + +Notice: different lidar use different serial_baudrate. + +## RPLIDAR frame + +RPLIDAR frame must be broadcasted according to picture shown in rplidar-frame.png diff --git a/launch/sllidar_a1_launch.py b/launch/sllidar_a1_launch.py new file mode 100644 index 0000000..b1ad70f --- /dev/null +++ b/launch/sllidar_a1_launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate}], + output='screen'), + ]) + diff --git a/launch/sllidar_a2m12_launch .py b/launch/sllidar_a2m12_launch .py new file mode 100644 index 0000000..f813a73 --- /dev/null +++ b/launch/sllidar_a2m12_launch .py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate}], + output='screen'), + ]) + diff --git a/launch/sllidar_a2m7_launch.py b/launch/sllidar_a2m7_launch.py new file mode 100644 index 0000000..f813a73 --- /dev/null +++ b/launch/sllidar_a2m7_launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate}], + output='screen'), + ]) + diff --git a/launch/sllidar_a2m8_launch.py b/launch/sllidar_a2m8_launch.py new file mode 100644 index 0000000..b1ad70f --- /dev/null +++ b/launch/sllidar_a2m8_launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate}], + output='screen'), + ]) + diff --git a/launch/sllidar_a3_launch.py b/launch/sllidar_a3_launch.py new file mode 100644 index 0000000..5125750 --- /dev/null +++ b/launch/sllidar_a3_launch.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') #for A3 is 256000 + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + ]) + diff --git a/launch/sllidar_c1_launch.py b/launch/sllidar_c1_launch.py new file mode 100644 index 0000000..90b01af --- /dev/null +++ b/launch/sllidar_c1_launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Standard') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + ]) + diff --git a/launch/sllidar_s1_launch.py b/launch/sllidar_s1_launch.py new file mode 100644 index 0000000..b11e7da --- /dev/null +++ b/launch/sllidar_s1_launch.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate}], + output='screen'), + ]) + diff --git a/launch/sllidar_s1_tcp_launch.py b/launch/sllidar_s1_tcp_launch.py new file mode 100644 index 0000000..f882dc5 --- /dev/null +++ b/launch/sllidar_s1_tcp_launch.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='tcp') + tcp_ip = LaunchConfiguration('tcp_ip', default='192.168.0.7') + tcp_port = LaunchConfiguration('tcp_port', default='20108') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'tcp_ip', + default_value=tcp_ip, + description='Specifying tcp ip to connected lidar'), + + DeclareLaunchArgument( + 'tcp_port', + default_value=tcp_port, + description='Specifying tcp port to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type': channel_type, + 'tcp_ip': tcp_ip, + 'tcp_port': tcp_port, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate}], + output='screen'), + ]) + + diff --git a/launch/sllidar_s2_launch.py b/launch/sllidar_s2_launch.py new file mode 100644 index 0000000..5cc7b85 --- /dev/null +++ b/launch/sllidar_s2_launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='1000000') #for s2 is 1000000 + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='DenseBoost') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + ]) + diff --git a/launch/sllidar_s2e_launch.py b/launch/sllidar_s2e_launch.py new file mode 100644 index 0000000..7da57c8 --- /dev/null +++ b/launch/sllidar_s2e_launch.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='udp') + udp_ip = LaunchConfiguration('udp_ip', default='192.168.11.2') + udp_port = LaunchConfiguration('udp_port', default='8089') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + scan_frequency = LaunchConfiguration('scan_frequency', default='10') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'udp_ip', + default_value=udp_ip, + description='Specifying udp ip to connected lidar'), + + DeclareLaunchArgument( + 'udp_port', + default_value=udp_port, + description='Specifying udp port to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type': channel_type, + 'udp_ip': udp_ip, + 'udp_port': udp_port, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + ]) + diff --git a/launch/sllidar_s3_launch.py b/launch/sllidar_s3_launch.py new file mode 100644 index 0000000..a2eebcd --- /dev/null +++ b/launch/sllidar_s3_launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='1000000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='DenseBoost') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + ]) + diff --git a/launch/sllidar_t1_launch.py b/launch/sllidar_t1_launch.py new file mode 100644 index 0000000..e2baccc --- /dev/null +++ b/launch/sllidar_t1_launch.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='udp') + udp_ip = LaunchConfiguration('udp_ip', default='192.168.11.2') + udp_port = LaunchConfiguration('udp_port', default='8089') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'udp_ip', + default_value=udp_ip, + description='Specifying udp ip to connected lidar'), + + DeclareLaunchArgument( + 'udp_port', + default_value=udp_port, + description='Specifying udp port to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type': channel_type, + 'udp_ip': udp_ip, + 'udp_port': udp_port, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + ]) + diff --git a/launch/view_sllidar_a1_launch.py b/launch/view_sllidar_a1_launch.py new file mode 100644 index 0000000..0959e7b --- /dev/null +++ b/launch/view_sllidar_a1_launch.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_a2m12_launch.py b/launch/view_sllidar_a2m12_launch.py new file mode 100644 index 0000000..40b9260 --- /dev/null +++ b/launch/view_sllidar_a2m12_launch.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_a2m7_launch.py b/launch/view_sllidar_a2m7_launch.py new file mode 100644 index 0000000..40b9260 --- /dev/null +++ b/launch/view_sllidar_a2m7_launch.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_a2m8_launch.py b/launch/view_sllidar_a2m8_launch.py new file mode 100644 index 0000000..0959e7b --- /dev/null +++ b/launch/view_sllidar_a2m8_launch.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_a3_launch.py b/launch/view_sllidar_a3_launch.py new file mode 100644 index 0000000..3949ec7 --- /dev/null +++ b/launch/view_sllidar_a3_launch.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') #for A3 is 256000 + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_c1_launch.py b/launch/view_sllidar_c1_launch.py new file mode 100644 index 0000000..cbba3bc --- /dev/null +++ b/launch/view_sllidar_c1_launch.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Standard') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode + }], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_s1_launch.py b/launch/view_sllidar_s1_launch.py new file mode 100644 index 0000000..70f2fa9 --- /dev/null +++ b/launch/view_sllidar_s1_launch.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='256000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate + }], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_s1_tcp_launch.py b/launch/view_sllidar_s1_tcp_launch.py new file mode 100644 index 0000000..1714da4 --- /dev/null +++ b/launch/view_sllidar_s1_tcp_launch.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='tcp') + tcp_ip = LaunchConfiguration('tcp_ip', default='192.168.0.7') + tcp_port = LaunchConfiguration('tcp_port', default='20108') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'tcp_ip', + default_value=tcp_ip, + description='Specifying tcp ip to connected lidar'), + + DeclareLaunchArgument( + 'tcp_port', + default_value=tcp_port, + description='Specifying tcp port to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type': channel_type, + 'tcp_ip': tcp_ip, + 'tcp_port': tcp_port, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + + diff --git a/launch/view_sllidar_s2_launch.py b/launch/view_sllidar_s2_launch.py new file mode 100644 index 0000000..016852c --- /dev/null +++ b/launch/view_sllidar_s2_launch.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='1000000') #for s2 is 1000000 + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='DenseBoost') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode + }], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_s2e_launch.py b/launch/view_sllidar_s2e_launch.py new file mode 100644 index 0000000..00a2b33 --- /dev/null +++ b/launch/view_sllidar_s2e_launch.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='udp') + udp_ip = LaunchConfiguration('udp_ip', default='192.168.11.2') + udp_port = LaunchConfiguration('udp_port', default='8089') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + scan_frequency = LaunchConfiguration('scan_frequency', default='10') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'udp_ip', + default_value=udp_ip, + description='Specifying udp ip to connected lidar'), + + DeclareLaunchArgument( + 'udp_port', + default_value=udp_port, + description='Specifying udp port to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type': channel_type, + 'udp_ip': udp_ip, + 'udp_port': udp_port, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_s3_launch.py b/launch/view_sllidar_s3_launch.py new file mode 100644 index 0000000..a2a15fe --- /dev/null +++ b/launch/view_sllidar_s3_launch.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='serial') + serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') + serial_baudrate = LaunchConfiguration('serial_baudrate', default='1000000') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='DenseBoost') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + + return LaunchDescription([ + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'serial_port', + default_value=serial_port, + description='Specifying usb port to connected lidar'), + + DeclareLaunchArgument( + 'serial_baudrate', + default_value=serial_baudrate, + description='Specifying usb port baudrate to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type':channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode + }], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/launch/view_sllidar_t1_launch.py b/launch/view_sllidar_t1_launch.py new file mode 100644 index 0000000..72c89ba --- /dev/null +++ b/launch/view_sllidar_t1_launch.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + channel_type = LaunchConfiguration('channel_type', default='udp') + udp_ip = LaunchConfiguration('udp_ip', default='192.168.11.2') + udp_port = LaunchConfiguration('udp_port', default='8089') + frame_id = LaunchConfiguration('frame_id', default='laser') + inverted = LaunchConfiguration('inverted', default='false') + angle_compensate = LaunchConfiguration('angle_compensate', default='true') + scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + + rviz_config_dir = os.path.join( + get_package_share_directory('sllidar_ros2'), + 'rviz', + 'sllidar_ros2.rviz') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'channel_type', + default_value=channel_type, + description='Specifying channel type of lidar'), + + DeclareLaunchArgument( + 'udp_ip', + default_value=udp_ip, + description='Specifying udp ip to connected lidar'), + + DeclareLaunchArgument( + 'udp_port', + default_value=udp_port, + description='Specifying udp port to connected lidar'), + + DeclareLaunchArgument( + 'frame_id', + default_value=frame_id, + description='Specifying frame_id of lidar'), + + DeclareLaunchArgument( + 'inverted', + default_value=inverted, + description='Specifying whether or not to invert scan data'), + + DeclareLaunchArgument( + 'angle_compensate', + default_value=angle_compensate, + description='Specifying whether or not to enable angle_compensate of scan data'), + + DeclareLaunchArgument( + 'scan_mode', + default_value=scan_mode, + description='Specifying scan mode of lidar'), + + Node( + package='sllidar_ros2', + executable='sllidar_node', + name='sllidar_node', + parameters=[{'channel_type': channel_type, + 'udp_ip': udp_ip, + 'udp_port': udp_port, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + output='screen'), + ]) + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..c90ef4d --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + sllidar_ros2 + 1.0.1 + + The rplidar ros2 package, support rplidar A2/A1 and A3/S1 + + Slamtec ROS Maintainer + BSD + ament_cmake + rclcpp + std_srvs + sensor_msgs + + ament_cmake + + diff --git a/rplidar_A1.png b/rplidar_A1.png new file mode 100644 index 0000000..994def4 Binary files /dev/null and b/rplidar_A1.png differ diff --git a/rplidar_A2.png b/rplidar_A2.png new file mode 100644 index 0000000..3ca9294 Binary files /dev/null and b/rplidar_A2.png differ diff --git a/rviz/sllidar_ros2.rviz b/rviz/sllidar_ros2.rviz new file mode 100644 index 0000000..51f93ea --- /dev/null +++ b/rviz/sllidar_ros2.rviz @@ -0,0 +1,159 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LaserScan1 + - /LaserScan1/Status1 + - /LaserScan1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0 + Min Value: 0 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 47 + Min Color: 0; 0; 0 + Min Intensity: 47 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: System Default + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: laser + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 16.872995376586914 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 72 + Y: 60 diff --git a/scripts/create_udev_rules.sh b/scripts/create_udev_rules.sh new file mode 100644 index 0000000..b1645d0 --- /dev/null +++ b/scripts/create_udev_rules.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +echo "remap the device serial port(ttyUSBX) to rplidar" +echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB" +echo "start copy rplidar.rules to /etc/udev/rules.d/" +colcon_cd rplidar_ros2 +sudo cp scripts/rplidar.rules /etc/udev/rules.d +echo " " +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart +echo "finish " diff --git a/scripts/delete_udev_rules.sh b/scripts/delete_udev_rules.sh new file mode 100644 index 0000000..05c77f8 --- /dev/null +++ b/scripts/delete_udev_rules.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +echo "delete remap the device serial port(ttyUSBX) to rplidar" +echo "sudo rm /etc/udev/rules.d/rplidar.rules" +sudo rm /etc/udev/rules.d/rplidar.rules +echo " " +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart +echo "finish delete" diff --git a/scripts/rplidar.rules b/scripts/rplidar.rules new file mode 100644 index 0000000..d75ed81 --- /dev/null +++ b/scripts/rplidar.rules @@ -0,0 +1,4 @@ +# set the udev rule , make the device_port be fixed by rplidar +# +KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar" + diff --git a/sdk/Makefile b/sdk/Makefile new file mode 100644 index 0000000..e1724b8 --- /dev/null +++ b/sdk/Makefile @@ -0,0 +1,77 @@ +#/* +# * RPLIDAR SDK +# * +# * Copyright (c) 2009 - 2014 RoboPeak Team +# * http://www.robopeak.com +# * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. +# * http://www.slamtec.com +# * +# */ +#/* +# * Redistribution and use in source and binary forms, with or without +# * modification, are permitted provided that the following conditions are met: +# * +# * 1. Redistributions of source code must retain the above copyright notice, +# * this list of conditions and the following disclaimer. +# * +# * 2. Redistributions in binary form must reproduce the above copyright notice, +# * this list of conditions and the following disclaimer in the documentation +# * and/or other materials provided with the distribution. +# * +# * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR +# * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +# * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +# * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +# * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +# * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +# * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, +# * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# * +# */ +# +HOME_TREE := ../ + +MODULE_NAME := $(notdir $(CURDIR)) + +include $(HOME_TREE)/mak_def.inc + +CXXSRC += src/sl_lidar_driver.cpp \ + src/hal/thread.cpp\ + src/sl_crc.cpp\ + src/sl_serial_channel.cpp\ + src/sl_lidarprotocol_codec.cpp\ + src/sl_async_transceiver.cpp\ + src/sl_tcp_channel.cpp\ + src/sl_udp_channel.cpp + + +C_INCLUDES += -I$(CURDIR)/include -I$(CURDIR)/src + + +#dataunpacker +CXXSRC += $(shell find $(CURDIR)/src/dataunpacker/ -name "*.cpp") + +C_INCLUDES += -I$(CURDIR)/src/dataunpacker -I$(CURDIR)/src/dataunpacker/unpacker + + +ifeq ($(BUILD_TARGET_PLATFORM),Linux) +CXXSRC += src/arch/linux/net_serial.cpp \ + src/arch/linux/net_socket.cpp \ + src/arch/linux/timer.cpp +endif + + +ifeq ($(BUILD_TARGET_PLATFORM),Darwin) +CXXSRC += src/arch/macOS/net_serial.cpp \ + src/arch/macOS/net_socket.cpp \ + src/arch/macOS/timer.cpp +endif + +all: build_sdk + +include $(HOME_TREE)/mak_common.inc + +clean: clean_sdk diff --git a/sdk/include/rplidar.h b/sdk/include/rplidar.h new file mode 100644 index 0000000..c4f4752 --- /dev/null +++ b/sdk/include/rplidar.h @@ -0,0 +1,45 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include "hal/types.h" +#include "rplidar_protocol.h" +#include "rplidar_cmd.h" + +#include "rplidar_driver.h" + +#define RPLIDAR_SDK_VERSION "2.0.0" +#define SLAMTEC_LIDAR_SDK_VERSION SL_LIDAR_SDK_VERSION diff --git a/sdk/include/rplidar_cmd.h b/sdk/include/rplidar_cmd.h new file mode 100644 index 0000000..6bfc7e7 --- /dev/null +++ b/sdk/include/rplidar_cmd.h @@ -0,0 +1,215 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014-2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_cmd.h" +#include "rplidar_protocol.h" + +// Commands +//----------------------------------------- + +#define RPLIDAR_AUTOBAUD_MAGICBYTE SL_LIDAR_AUTOBAUD_MAGICBYTE + +// Commands without payload and response +#define RPLIDAR_CMD_STOP SL_LIDAR_CMD_STOP +#define RPLIDAR_CMD_SCAN SL_LIDAR_CMD_SCAN +#define RPLIDAR_CMD_FORCE_SCAN SL_LIDAR_CMD_FORCE_SCAN +#define RPLIDAR_CMD_RESET SL_LIDAR_CMD_RESET + + +// Commands without payload but have response +#define RPLIDAR_CMD_GET_DEVICE_INFO SL_LIDAR_CMD_GET_DEVICE_INFO +#define RPLIDAR_CMD_GET_DEVICE_HEALTH SL_LIDAR_CMD_GET_DEVICE_HEALTH + +#define RPLIDAR_CMD_GET_SAMPLERATE SL_LIDAR_CMD_GET_SAMPLERATE //added in fw 1.17 + +#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL + +// Commands with payload but no response +#define RPLIDAR_CMD_NEW_BAUDRATE_CONFIRM SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM //added in fw 1.30 + +// Commands with payload and have response +#define RPLIDAR_CMD_EXPRESS_SCAN SL_LIDAR_CMD_EXPRESS_SCAN //added in fw 1.17 +#define RPLIDAR_CMD_HQ_SCAN SL_LIDAR_CMD_HQ_SCAN //added in fw 1.24 +#define RPLIDAR_CMD_GET_LIDAR_CONF SL_LIDAR_CMD_GET_LIDAR_CONF //added in fw 1.24 +#define RPLIDAR_CMD_SET_LIDAR_CONF SL_LIDAR_CMD_SET_LIDAR_CONF //added in fw 1.24 +//add for A2 to set RPLIDAR motor pwm when using accessory board +#define RPLIDAR_CMD_SET_MOTOR_PWM SL_LIDAR_CMD_SET_MOTOR_PWM +#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG SL_LIDAR_CMD_GET_ACC_BOARD_FLAG + +#if defined(_WIN32) +#pragma pack(1) +#endif + + +// Payloads +// ------------------------------------------ +#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL +#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE // won't been supported but keep to prevent build fail +//for express working flag(extending express scan protocol) +#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST +#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION + +//for ultra express working flag +#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD +#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY + +#define RPLIDAR_HQ_SCAN_FLAG_CCW (0x1<<0) +#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER (0x1<<1) +#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE (0x1<<2) + +typedef sl_lidar_payload_express_scan_t rplidar_payload_express_scan_t; +typedef sl_lidar_payload_hq_scan_t rplidar_payload_hq_scan_t; +typedef sl_lidar_payload_get_scan_conf_t rplidar_payload_get_scan_conf_t; +typedef sl_lidar_payload_motor_pwm_t rplidar_payload_motor_pwm_t; +typedef sl_lidar_payload_acc_board_flag_t rplidar_payload_acc_board_flag_t; +typedef sl_lidar_payload_set_scan_conf_t rplidar_payload_set_scan_conf_t; +typedef sl_lidar_payload_new_bps_confirmation_t rplidar_payload_new_bps_confirmation_t; + +// Response +// ------------------------------------------ +#define RPLIDAR_ANS_TYPE_DEVINFO SL_LIDAR_ANS_TYPE_DEVINFO +#define RPLIDAR_ANS_TYPE_DEVHEALTH SL_LIDAR_ANS_TYPE_DEVHEALTH +#define RPLIDAR_ANS_TYPE_MEASUREMENT SL_LIDAR_ANS_TYPE_MEASUREMENT +// Added in FW ver 1.17 +#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED +#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ +// Added in FW ver 1.17 +#define RPLIDAR_ANS_TYPE_SAMPLE_RATE SL_LIDAR_ANS_TYPE_SAMPLE_RATE +//added in FW ver 1.23alpha +#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA +//added in FW ver 1.24 +#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF +#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF +#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED +#define RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED + +#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG + +#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK + +typedef sl_lidar_response_acc_board_flag_t rplidar_response_acc_board_flag_t; + + +#define RPLIDAR_STATUS_OK SL_LIDAR_STATUS_OK +#define RPLIDAR_STATUS_WARNING SL_LIDAR_STATUS_WARNING +#define RPLIDAR_STATUS_ERROR SL_LIDAR_STATUS_ERROR + +#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT SL_LIDAR_RESP_MEASUREMENT_SYNCBIT +#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT +#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT SL_LIDAR_RESP_HQ_FLAG_SYNCBIT +#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT SL_LIDAR_RESP_MEASUREMENT_CHECKBIT +#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT + +typedef sl_lidar_response_sample_rate_t rplidar_response_sample_rate_t; +typedef sl_lidar_response_measurement_node_t rplidar_response_measurement_node_t; + +//[distance_sync flags] +#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK +#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK + +typedef sl_lidar_response_cabin_nodes_t rplidar_response_cabin_nodes_t; + + +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1 +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2 +#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT + + +typedef sl_lidar_response_capsule_measurement_nodes_t rplidar_response_capsule_measurement_nodes_t; +typedef sl_lidar_response_dense_cabin_nodes_t rplidar_response_dense_cabin_nodes_t; +typedef sl_lidar_response_dense_capsule_measurement_nodes_t rplidar_response_dense_capsule_measurement_nodes_t; +typedef sl_lidar_response_ultra_dense_capsule_measurement_nodes_t rplidar_response_ultra_dense_capsule_measurement_nodes_t; +// ext1 : x2 boost mode + +#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS +#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS + +typedef sl_lidar_response_ultra_cabin_nodes_t rplidar_response_ultra_cabin_nodes_t; +typedef sl_lidar_response_ultra_capsule_measurement_nodes_t rplidar_response_ultra_capsule_measurement_nodes_t; +typedef sl_lidar_response_measurement_node_hq_t rplidar_response_measurement_node_hq_t; +typedef sl_lidar_response_hq_capsule_measurement_nodes_t rplidar_response_hq_capsule_measurement_nodes_t; + + +# define RPLIDAR_CONF_SCAN_COMMAND_STD SL_LIDAR_CONF_SCAN_COMMAND_STD +# define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS +# define RPLIDAR_CONF_SCAN_COMMAND_HQ SL_LIDAR_CONF_SCAN_COMMAND_HQ +# define RPLIDAR_CONF_SCAN_COMMAND_BOOST SL_LIDAR_CONF_SCAN_COMMAND_BOOST +# define RPLIDAR_CONF_SCAN_COMMAND_STABILITY SL_LIDAR_CONF_SCAN_COMMAND_STABILITY +# define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY + +#define RPLIDAR_CONF_ANGLE_RANGE SL_LIDAR_CONF_ANGLE_RANGE +#define RPLIDAR_CONF_DESIRED_ROT_FREQ SL_LIDAR_CONF_DESIRED_ROT_FREQ +#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP SL_LIDAR_CONF_SCAN_COMMAND_BITMAP +#define RPLIDAR_CONF_MIN_ROT_FREQ SL_LIDAR_CONF_MIN_ROT_FREQ +#define RPLIDAR_CONF_MAX_ROT_FREQ SL_LIDAR_CONF_MAX_ROT_FREQ +#define RPLIDAR_CONF_MAX_DISTANCE SL_LIDAR_CONF_MAX_DISTANCE + +#define RPLIDAR_CONF_SCAN_MODE_COUNT SL_LIDAR_CONF_SCAN_MODE_COUNT +#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE +#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE +#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE +#define RPLIDAR_CONF_SCAN_MODE_TYPICAL SL_LIDAR_CONF_SCAN_MODE_TYPICAL +#define RPLIDAR_CONF_SCAN_MODE_NAME SL_LIDAR_CONF_SCAN_MODE_NAME +#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP +#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP +#define RPLIDAR_CONF_LIDAR_STATIC_IP_ADDR SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR +#define RPLIDAR_CONF_LIDAR_MAC_ADDR SL_LIDAR_CONF_LIDAR_MAC_ADDR + +#define RPLIDAR_CONF_DETECTED_SERIAL_BPS SL_LIDAR_CONF_DETECTED_SERIAL_BPS + +typedef sl_lidar_response_get_lidar_conf_t rplidar_response_get_lidar_conf_t; +typedef sl_lidar_response_set_lidar_conf_t rplidar_response_set_lidar_conf_t; +typedef sl_lidar_response_device_info_t rplidar_response_device_info_t; +typedef sl_lidar_response_device_health_t rplidar_response_device_health_t; +typedef sl_lidar_ip_conf_t rplidar_ip_conf_t; +typedef sl_lidar_response_device_macaddr_info_t rplidar_response_device_macaddr_info_t; + +// Definition of the variable bit scale encoding mechanism +#define RPLIDAR_VARBITSCALE_X2_SRC_BIT SL_LIDAR_VARBITSCALE_X2_SRC_BIT +#define RPLIDAR_VARBITSCALE_X4_SRC_BIT SL_LIDAR_VARBITSCALE_X4_SRC_BIT +#define RPLIDAR_VARBITSCALE_X8_SRC_BIT SL_LIDAR_VARBITSCALE_X8_SRC_BIT +#define RPLIDAR_VARBITSCALE_X16_SRC_BIT SL_LIDAR_VARBITSCALE_X16_SRC_BIT + +#define RPLIDAR_VARBITSCALE_X2_DEST_VAL SL_LIDAR_VARBITSCALE_X2_DEST_VAL +#define RPLIDAR_VARBITSCALE_X4_DEST_VAL SL_LIDAR_VARBITSCALE_X4_DEST_VAL +#define RPLIDAR_VARBITSCALE_X8_DEST_VAL SL_LIDAR_VARBITSCALE_X8_DEST_VAL +#define RPLIDAR_VARBITSCALE_X16_DEST_VAL SL_LIDAR_VARBITSCALE_X16_DEST_VAL + +#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h new file mode 100644 index 0000000..bd2fc82 --- /dev/null +++ b/sdk/include/rplidar_driver.h @@ -0,0 +1,247 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" + +#ifndef __cplusplus +#error "The RPlidar SDK requires a C++ compiler to be built" +#endif + + +namespace rp { namespace standalone{ namespace rplidar { + using namespace sl; + typedef LidarScanMode RplidarScanMode; + +enum { + DRIVER_TYPE_SERIALPORT = 0x0, + DRIVER_TYPE_TCP = 0x1, + DRIVER_TYPE_UDP = 0x2, +}; + +class RPlidarDriver { +public: + enum { + DEFAULT_TIMEOUT = 2000, //2000 ms + }; + + enum { + MAX_SCAN_NODES = 8192, + }; + + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + +public: + /// Create an RPLIDAR Driver Instance + /// This interface should be invoked first before any other operations + /// + /// \param drivertype the connection type used by the driver. + static RPlidarDriver * CreateDriver(_u32 drivertype = CHANNEL_TYPE_SERIALPORT); + + + RPlidarDriver(sl_u32 channelType); + + /// Dispose the RPLIDAR Driver Instance specified by the drv parameter + /// Applications should invoke this interface when the driver instance is no longer used in order to free memory + static void DisposeDriver(RPlidarDriver * drv); + + /// Open the specified serial port and connect to a target RPLIDAR device + /// + /// \param port_path the device path of the serial port + /// e.g. on Windows, it may be com3 or \\.\com10 + /// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc + /// + /// \param baudrate the baudrate used + /// For most RPLIDAR models, the baudrate should be set to 115200 + /// + /// \param flag other flags + /// Reserved for future use, always set to Zero + u_result connect(const char *path, _u32 portOrBaud, _u32 flag = 0); + + /// Disconnect with the RPLIDAR and close the serial port + void disconnect(); + + /// Returns TRUE when the connection has been established + bool isConnected(); + + /// Ask the RPLIDAR core system to reset it self + /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode. + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result reset(_u32 timeout = DEFAULT_TIMEOUT); + + u_result clearNetSerialRxCache() { + return RESULT_OK; + } + // FW1.24 + /// Get all scan modes that supported by lidar + u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// Get typical scan mode of lidar + u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// Start scan + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL); + + /// Start scan in specific mode + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT); + + /// Retrieve the health status of the RPLIDAR + /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. + /// + /// \param health The health status info returned from the RPLIDAR + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); + + /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. + /// + /// \param info The device information returned from the RPLIDAR + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); + + /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only. + /// + /// \param pwm The motor pwm value would like to set + u_result setMotorPWM(_u16 pwm); + + /// Start RPLIDAR's motor when using accessory board + u_result startMotor(); + + /// Stop RPLIDAR's motor when using accessory board + u_result stopMotor(); + + /// Check whether the device support motor control. + /// Note: this API will disable grab. + /// + /// \param support Return the result. + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); + + ///Set LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + u_result setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT); + + ///Get LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + u_result getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT); + + ///Get LPX and S2E series lidar's MAC address + /// + /// \param macAddrArray The device MAC information returned from the LPX series lidar + u_result getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result stop(_u32 timeout = DEFAULT_TIMEOUT); + + /// Wait and grab a complete 0-360 degree scan data previously received. + /// The grabbed scan data returned by this interface always has the following charactistics: + /// + /// 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 + /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan + /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \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). + /// Once the interface returns, this parameter will store the actual received data count. + /// + /// \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. + /// + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. + /// + /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. + u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); + + /// Ascending the scan data according to the angle value in the scan. + /// + /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData + /// + /// \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). + /// Once the interface returns, this parameter will store the actual received data count. + /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. + u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count); + + /// Return received scan points even if it's not complete scan + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count Once the interface returns, this parameter will store the actual received data count. + /// + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count); + + /// Return received scan points even if it's not complete scan + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count Once the interface returns, this parameter will store the actual received data count. + /// + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count); + + + virtual ~RPlidarDriver(); +protected: + RPlidarDriver(); + +private: + sl_u32 _channelType; + IChannel* _channel; + ILidarDriver* _lidarDrv; + +}; + + + + +}}} diff --git a/sdk/include/rplidar_protocol.h b/sdk/include/rplidar_protocol.h new file mode 100644 index 0000000..cddb9c8 --- /dev/null +++ b/sdk/include/rplidar_protocol.h @@ -0,0 +1,61 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_protocol.h" +// RP-Lidar Input Packets + +#define RPLIDAR_CMD_SYNC_BYTE SL_LIDAR_CMD_SYNC_BYTE +#define RPLIDAR_CMDFLAG_HAS_PAYLOAD SL_LIDAR_CMDFLAG_HAS_PAYLOAD + + +#define RPLIDAR_ANS_SYNC_BYTE1 SL_LIDAR_ANS_SYNC_BYTE1 +#define RPLIDAR_ANS_SYNC_BYTE2 SL_LIDAR_ANS_SYNC_BYTE2 + +#define RPLIDAR_ANS_PKTFLAG_LOOP SL_LIDAR_ANS_PKTFLAG_LOOP + +#define RPLIDAR_ANS_HEADER_SIZE_MASK SL_LIDAR_ANS_HEADER_SIZE_MASK +#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT + +#if defined(_WIN32) +#pragma pack(1) +#endif + +typedef sl_lidar_cmd_packet_t rplidar_cmd_packet_t; +typedef sl_lidar_ans_header_t rplidar_ans_header_t; + + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/sdk/include/rptypes.h b/sdk/include/rptypes.h new file mode 100644 index 0000000..34d8ecb --- /dev/null +++ b/sdk/include/rptypes.h @@ -0,0 +1,116 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + + +#ifdef _WIN32 + +//fake stdint.h for VC only + +typedef signed char int8_t; +typedef unsigned char uint8_t; + +typedef __int16 int16_t; +typedef unsigned __int16 uint16_t; + +typedef __int32 int32_t; +typedef unsigned __int32 uint32_t; + +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; + +#else + +#include + +#endif + + +//based on stdint.h +typedef int8_t _s8; +typedef uint8_t _u8; + +typedef int16_t _s16; +typedef uint16_t _u16; + +typedef int32_t _s32; +typedef uint32_t _u32; + +typedef int64_t _s64; +typedef uint64_t _u64; + +#define __small_endian + +#ifndef __GNUC__ +#define __attribute__(x) +#endif + + +// The _word_size_t uses actual data bus width of the current CPU +#ifdef _AVR_ +typedef _u8 _word_size_t; +#define THREAD_PROC +#elif defined (WIN64) +typedef _u64 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (WIN32) +typedef _u32 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (__GNUC__) +typedef unsigned long _word_size_t; +#define THREAD_PROC +#elif defined (__ICCARM__) +typedef _u32 _word_size_t; +#define THREAD_PROC +#endif + + +typedef uint32_t u_result; + +#define RESULT_OK 0 +#define RESULT_FAIL_BIT 0x80000000 +#define RESULT_ALREADY_DONE 0x20 +#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) +#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) +#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) + +#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) +#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) + +typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); diff --git a/sdk/include/sl_crc.h b/sdk/include/sl_crc.h new file mode 100644 index 0000000..184a8df --- /dev/null +++ b/sdk/include/sl_crc.h @@ -0,0 +1,43 @@ +/* +* Slamtec LIDAR SDK +* +* sl_crc.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_cmd.h" + +namespace sl {namespace crc32 { + sl_u32 bitrev(sl_u32 input, sl_u16 bw);//reflect + void init(sl_u32 poly); // table init + sl_u32 cal(sl_u32 crc, void* input, sl_u16 len); + sl_result getResult(sl_u8 *ptr, sl_u32 len); +}} diff --git a/sdk/include/sl_lidar.h b/sdk/include/sl_lidar.h new file mode 100644 index 0000000..db15fe7 --- /dev/null +++ b/sdk/include/sl_lidar.h @@ -0,0 +1,47 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_driver.h" + +#define SL_LIDAR_SDK_VERSION_MAJOR 2 +#define SL_LIDAR_SDK_VERSION_MINOR 1 +#define SL_LIDAR_SDK_VERSION_PATCH 0 +#define SL_LIDAR_SDK_VERSION_SEQ ((SL_LIDAR_SDK_VERSION_MAJOR << 16) | (SL_LIDAR_SDK_VERSION_MINOR << 8) | SL_LIDAR_SDK_VERSION_PATCH) + + +#define SL_LIDAR_SDK_VERSION_MK_STR_INDIR(x) #x +#define SL_LIDAR_SDK_VERSION_MK_STR(x) SL_LIDAR_SDK_VERSION_MK_STR_INDIR(x) + +#define SL_LIDAR_SDK_VERSION (SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MAJOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MINOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_PATCH)) diff --git a/sdk/include/sl_lidar_cmd.h b/sdk/include/sl_lidar_cmd.h new file mode 100644 index 0000000..bd3f8d2 --- /dev/null +++ b/sdk/include/sl_lidar_cmd.h @@ -0,0 +1,388 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar_cmd.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable:4200) +#endif + +#include "sl_lidar_protocol.h" + + // Commands + //----------------------------------------- + + +#define SL_LIDAR_AUTOBAUD_MAGICBYTE 0x41 + + // Commands without payload and response +#define SL_LIDAR_CMD_STOP 0x25 +#define SL_LIDAR_CMD_SCAN 0x20 +#define SL_LIDAR_CMD_FORCE_SCAN 0x21 +#define SL_LIDAR_CMD_RESET 0x40 + +// Commands with payload but no response +#define SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM 0x90 //added in fw 1.30 + +// Commands without payload but have response +#define SL_LIDAR_CMD_GET_DEVICE_INFO 0x50 +#define SL_LIDAR_CMD_GET_DEVICE_HEALTH 0x52 + +#define SL_LIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17 + +#define SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8 + + +// Commands with payload and have response +#define SL_LIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 +#define SL_LIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24 +#define SL_LIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24 +#define SL_LIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24 +//add for A2 to set RPLIDAR motor pwm when using accessory board +#define SL_LIDAR_CMD_SET_MOTOR_PWM 0xF0 +#define SL_LIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF + +#if defined(_WIN32) +#pragma pack(1) +#endif + + +// Payloads +// ------------------------------------------ +#define SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL 0 +#define SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail +//for express working flag(extending express scan protocol) +#define SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001 +#define SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002 + +//for ultra express working flag +#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001 +#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002 + +typedef struct _sl_lidar_payload_express_scan_t +{ + sl_u8 working_mode; + sl_u16 working_flags; + sl_u16 param; +} __attribute__((packed)) sl_lidar_payload_express_scan_t; + +typedef struct _sl_lidar_payload_hq_scan_t +{ + sl_u8 flag; + sl_u8 reserved[32]; +} __attribute__((packed)) sl_lidar_payload_hq_scan_t; + +typedef struct _sl_lidar_payload_get_scan_conf_t +{ + sl_u32 type; +} __attribute__((packed)) sl_lidar_payload_get_scan_conf_t; + +typedef struct _sl_payload_set_scan_conf_t { + sl_u32 type; +} __attribute__((packed)) sl_lidar_payload_set_scan_conf_t; + + +#define DEFAULT_MOTOR_SPEED (0xFFFFu) + +typedef struct _sl_lidar_payload_motor_pwm_t +{ + sl_u16 pwm_value; +} __attribute__((packed)) sl_lidar_payload_motor_pwm_t; + +typedef struct _sl_lidar_payload_acc_board_flag_t +{ + sl_u32 reserved; +} __attribute__((packed)) sl_lidar_payload_acc_board_flag_t; + +typedef struct _sl_lidar_payload_hq_spd_ctrl_t { + sl_u16 rpm; +} __attribute__((packed))sl_lidar_payload_hq_spd_ctrl_t; + + +typedef struct _sl_lidar_payload_new_bps_confirmation_t { + sl_u16 flag; // reserved, must be 0x5F5F + sl_u32 required_bps; + sl_u16 param; +} __attribute__((packed)) sl_lidar_payload_new_bps_confirmation_t; + +// Response +// ------------------------------------------ +#define SL_LIDAR_ANS_TYPE_DEVINFO 0x4 +#define SL_LIDAR_ANS_TYPE_DEVHEALTH 0x6 + +#define SL_LIDAR_ANS_TYPE_MEASUREMENT 0x81 +// Added in FW ver 1.17 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83 +//added in FW ver 1.23alpha +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED 0x85 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED 0x86 + + +// Added in FW ver 1.17 +#define SL_LIDAR_ANS_TYPE_SAMPLE_RATE 0x15 + +//added in FW ver 1.24 +#define SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20 +#define SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21 + + +#define SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF + +#define SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1) +typedef struct _sl_lidar_response_acc_board_flag_t +{ + sl_u32 support_flag; +} __attribute__((packed)) sl_lidar_response_acc_board_flag_t; + + +#define SL_LIDAR_STATUS_OK 0x0 +#define SL_LIDAR_STATUS_WARNING 0x1 +#define SL_LIDAR_STATUS_ERROR 0x2 + +#define SL_LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) +#define SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 + +#define SL_LIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0) + +#define SL_LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) +#define SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 + +typedef struct _sl_lidar_response_sample_rate_t +{ + sl_u16 std_sample_duration_us; + sl_u16 express_sample_duration_us; +} __attribute__((packed)) sl_lidar_response_sample_rate_t; + +typedef struct _sl_lidar_response_measurement_node_t +{ + sl_u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; + sl_u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; + sl_u16 distance_q2; +} __attribute__((packed)) sl_lidar_response_measurement_node_t; + +//[distance_sync flags] +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3) +#define SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC) + +typedef struct _sl_lidar_response_cabin_nodes_t +{ + sl_u16 distance_angle_1; // see [distance_sync flags] + sl_u16 distance_angle_2; // see [distance_sync flags] + sl_u8 offset_angles_q3; +} __attribute__((packed)) sl_lidar_response_cabin_nodes_t; + + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5 + +#define SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5 + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15) + +typedef struct _sl_lidar_response_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_cabin_nodes_t cabins[16]; +} __attribute__((packed)) sl_lidar_response_capsule_measurement_nodes_t; + +typedef struct _sl_lidar_response_dense_cabin_nodes_t +{ + sl_u16 distance; +} __attribute__((packed)) sl_lidar_response_dense_cabin_nodes_t; + +typedef struct _sl_lidar_response_dense_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_dense_cabin_nodes_t cabins[40]; +} __attribute__((packed)) sl_lidar_response_dense_capsule_measurement_nodes_t; + + +typedef struct _sl_lidar_response_ultra_dense_cabin_nodes_t { + sl_u16 qualityl_distance_scale[2]; + sl_u8 qualityh_array; +} __attribute__((packed)) sl_lidar_response_ultra_dense_cabin_nodes_t; + +typedef struct _sl_lidar_response_ultra_dense_capsule_measurement_nodes_t { + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u32 time_stamp; + sl_u16 dev_status; + sl_u16 start_angle_sync_q6; + sl_lidar_response_ultra_dense_cabin_nodes_t cabins[32]; +} __attribute__((packed)) sl_lidar_response_ultra_dense_capsule_measurement_nodes_t; + + +// ext1 : x2 boost mode + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12 +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10 + +typedef struct _sl_lidar_response_ultra_cabin_nodes_t +{ + // 31 0 + // | predict2 10bit | predict1 10bit | major 12bit | + sl_u32 combined_x3; +} __attribute__((packed)) sl_lidar_response_ultra_cabin_nodes_t; + +typedef struct _sl_lidar_response_ultra_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_ultra_cabin_nodes_t ultra_cabins[32]; +} __attribute__((packed)) sl_lidar_response_ultra_capsule_measurement_nodes_t; + +typedef struct sl_lidar_response_measurement_node_hq_t +{ + sl_u16 angle_z_q14; + sl_u32 dist_mm_q2; + sl_u8 quality; + sl_u8 flag; +} __attribute__((packed)) sl_lidar_response_measurement_node_hq_t; + +typedef struct _sl_lidar_response_hq_capsule_measurement_nodes_t +{ + sl_u8 sync_byte; + sl_u64 time_stamp; + sl_lidar_response_measurement_node_hq_t node_hq[96]; + sl_u32 crc32; +}__attribute__((packed)) sl_lidar_response_hq_capsule_measurement_nodes_t; + + +# define SL_LIDAR_CONF_SCAN_COMMAND_STD 0 +# define SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS 1 +# define SL_LIDAR_CONF_SCAN_COMMAND_HQ 2 +# define SL_LIDAR_CONF_SCAN_COMMAND_BOOST 3 +# define SL_LIDAR_CONF_SCAN_COMMAND_STABILITY 4 +# define SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5 + +#define SL_LIDAR_CONF_ANGLE_RANGE 0x00000000 +#define SL_LIDAR_CONF_DESIRED_ROT_FREQ 0x00000001 +#define SL_LIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002 +#define SL_LIDAR_CONF_MIN_ROT_FREQ 0x00000004 +#define SL_LIDAR_CONF_MAX_ROT_FREQ 0x00000005 +#define SL_LIDAR_CONF_MAX_DISTANCE 0x00000060 + +#define SL_LIDAR_CONF_SCAN_MODE_COUNT 0x00000070 +#define SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071 +#define SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074 +#define SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075 +#define SL_LIDAR_CONF_LIDAR_MAC_ADDR 0x00000079 +#define SL_LIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C +#define SL_LIDAR_CONF_SCAN_MODE_NAME 0x0000007F + + +#define SL_LIDAR_CONF_MODEL_REVISION_ID 0x00000080 +#define SL_LIDAR_CONF_MODEL_NAME_ALIAS 0x00000081 + +#define SL_LIDAR_CONF_DETECTED_SERIAL_BPS 0x000000A1 + +#define SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR 0x0001CCC0 +#define SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4 +#define SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5 + +typedef struct _sl_lidar_response_get_lidar_conf +{ + sl_u32 type; + sl_u8 payload[0]; +}__attribute__((packed)) sl_lidar_response_get_lidar_conf_t; + +typedef struct _sl_lidar_response_set_lidar_conf +{ + sl_u32 type; + sl_u32 result; +}__attribute__((packed)) sl_lidar_response_set_lidar_conf_t; + + +typedef struct _sl_lidar_response_device_info_t +{ + sl_u8 model; + sl_u16 firmware_version; + sl_u8 hardware_version; + sl_u8 serialnum[16]; +} __attribute__((packed)) sl_lidar_response_device_info_t; + +typedef struct _sl_lidar_response_device_health_t +{ + sl_u8 status; + sl_u16 error_code; +} __attribute__((packed)) sl_lidar_response_device_health_t; + +typedef struct _sl_lidar_ip_conf_t { + sl_u8 ip_addr[4]; + sl_u8 net_mask[4]; + sl_u8 gw[4]; +}__attribute__((packed)) sl_lidar_ip_conf_t; + +typedef struct _sl_lidar_response_device_macaddr_info_t { + sl_u8 macaddr[6]; +} __attribute__((packed)) sl_lidar_response_device_macaddr_info_t; + +typedef struct _sl_lidar_response_desired_rot_speed_t{ + sl_u16 rpm; + sl_u16 pwm_ref; +}__attribute__((packed)) sl_lidar_response_desired_rot_speed_t; + +// Definition of the variable bit scale encoding mechanism +#define SL_LIDAR_VARBITSCALE_X2_SRC_BIT 9 +#define SL_LIDAR_VARBITSCALE_X4_SRC_BIT 11 +#define SL_LIDAR_VARBITSCALE_X8_SRC_BIT 12 +#define SL_LIDAR_VARBITSCALE_X16_SRC_BIT 14 + +#define SL_LIDAR_VARBITSCALE_X2_DEST_VAL 512 +#define SL_LIDAR_VARBITSCALE_X4_DEST_VAL 1280 +#define SL_LIDAR_VARBITSCALE_X8_DEST_VAL 1792 +#define SL_LIDAR_VARBITSCALE_X16_DEST_VAL 3328 + +#define SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \ + ( (((0x1<<(_BITS_)) - SL_LIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \ + ((SL_LIDAR_VARBITSCALE_X16_DEST_VAL - SL_LIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \ + ((SL_LIDAR_VARBITSCALE_X8_DEST_VAL - SL_LIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \ + ((SL_LIDAR_VARBITSCALE_X4_DEST_VAL - SL_LIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \ + SL_LIDAR_VARBITSCALE_X2_DEST_VAL - 1) + + +#if defined(_WIN32) +#pragma pack() +#endif + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif \ No newline at end of file diff --git a/sdk/include/sl_lidar_driver.h b/sdk/include/sl_lidar_driver.h new file mode 100644 index 0000000..9dade3c --- /dev/null +++ b/sdk/include/sl_lidar_driver.h @@ -0,0 +1,569 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifndef __cplusplus +#error "The Slamtec LIDAR SDK requires a C++ compiler to be built" +#endif + +#include +#include +#include + +#ifndef DEPRECATED + #ifdef __GNUC__ + #define DEPRECATED(func) func __attribute__ ((deprecated)) + #elif defined(_MSC_VER) + #define DEPRECATED(func) __declspec(deprecated) func + #else + #pragma message("WARNING: You need to implement DEPRECATED for this compiler") + #define DEPRECATED(func) func + #endif +#endif + + +#include "sl_lidar_cmd.h" + +#include + +namespace sl { + +#ifdef DEPRECATED +#define DEPRECATED_WARN(fn, replacement) do { \ + static bool __shown__ = false; \ + if (!__shown__) { \ + printDeprecationWarn(fn, replacement); \ + __shown__ = true; \ + } \ + } while (0) +#endif + + /** + * Lidar scan mode + */ + struct LidarScanMode + { + // Mode id + sl_u16 id; + + // Time cost for one measurement (in microseconds) + float us_per_sample; + + // Max distance in this scan mode (in meters) + float max_distance; + + // The answer command code for this scan mode + sl_u8 ans_type; + + // The name of scan mode (padding with 0 if less than 64 characters) + char scan_mode[64]; + }; + + template + struct Result + { + sl_result err; + T value; + Result(const T& value) + : err(SL_RESULT_OK) + , value(value) + { + } + + Result(sl_result err) + : err(err) + , value() + { + } + + operator sl_result() const + { + return err; + } + + operator bool() const + { + return SL_IS_OK(err); + } + + T& operator* () + { + return value; + } + + T* operator-> () + { + return &value; + } + }; + + enum LIDARTechnologyType { + LIDAR_TECHNOLOGY_UNKNOWN = 0, + LIDAR_TECHNOLOGY_TRIANGULATION = 1, + LIDAR_TECHNOLOGY_DTOF = 2, + LIDAR_TECHNOLOGY_ETOF = 3, + LIDAR_TECHNOLOGY_FMCW = 4, + }; + + enum LIDARMajorType { + LIDAR_MAJOR_TYPE_UNKNOWN = 0, + LIDAR_MAJOR_TYPE_A_SERIES = 1, + LIDAR_MAJOR_TYPE_S_SERIES = 2, + LIDAR_MAJOR_TYPE_T_SERIES = 3, + LIDAR_MAJOR_TYPE_M_SERIES = 4, + LIDAR_MAJOR_TYPE_C_SERIES = 6, + }; + + enum LIDARInterfaceType { + LIDAR_INTERFACE_UART = 0, + LIDAR_INTERFACE_ETHERNET = 1, + LIDAR_INTERFACE_USB = 2, + LIDAR_INTERFACE_CANBUS = 5, + + + LIDAR_INTERFACE_UNKNOWN = 0xFFFF, + }; + + struct SlamtecLidarTimingDesc { + + sl_u32 sample_duration_uS; + sl_u32 native_baudrate; + + sl_u32 linkage_delay_uS; + + LIDARInterfaceType native_interface_type; + + bool native_timestamp_support; + }; + + /** + * Abstract interface of communication channel + */ + class IChannel + { + public: + virtual ~IChannel() {} + + public: + /** + * Open communication channel (return true if succeed) + */ + virtual bool open() = 0; + + /** + * Close communication channel + */ + virtual void close() = 0; + + /** + * Flush all written data to remote endpoint + */ + virtual void flush() = 0; + + /** + * Wait for some data + * \param size Bytes to wait + * \param timeoutInMs Wait timeout (in microseconds, -1 for forever) + * \param actualReady [out] actual ready bytes + * \return true for data ready + */ + virtual bool waitForData(size_t size, sl_u32 timeoutInMs = -1, size_t* actualReady = nullptr) = 0; + + + /** + * Wait for some data + * \param size_hint Byte count may available to retrieve without beening blocked + * \param timeoutInMs Wait timeout (in microseconds, -1 for forever) + * \return RESULT_OK if there is data available for receiving + * RESULT_OPERATION_TIMEOUT if the given timeout duration is exceed + * RESULT_OPERATION_FAIL if there is something wrong with the channel + */ + virtual sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs = 1000) = 0; + + + /** + * Send data to remote endpoint + * \param data The data buffer + * \param size The size of data buffer (in bytes) + * \return Bytes written (negative for write failure) + */ + virtual int write(const void* data, size_t size) = 0; + + /** + * Read data from the chanel + * \param buffer The buffer to receive data + * \param size The size of the read buffer + * \return Bytes read (negative for read failure) + */ + virtual int read(void* buffer, size_t size) = 0; + + /** + * Clear read cache + */ + virtual void clearReadCache() = 0; + + virtual int getChannelType() = 0; + + private: + + }; + + /** + * Abstract interface of serial port channel + */ + class ISerialPortChannel : public IChannel + { + public: + virtual ~ISerialPortChannel() {} + + public: + virtual void setDTR(bool dtr) = 0; + }; + + /** + * Create a serial channel + * \param device Serial port device + * e.g. on Windows, it may be com3 or \\.\com10 + * on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc + * \param baudrate Baudrate + * Please refer to the datasheet for the baudrate (maybe 115200 or 256000) + */ + Result createSerialPortChannel(const std::string& device, int baudrate); + + /** + * Create a TCP channel + * \param ip IP address of the device + * \param port TCP port + */ + Result createTcpChannel(const std::string& ip, int port); + + /** + * Create a UDP channel + * \param ip IP address of the device + * \param port UDP port + */ + Result createUdpChannel(const std::string& ip, int port); + + enum MotorCtrlSupport + { + MotorCtrlSupportNone = 0, + MotorCtrlSupportPwm = 1, + MotorCtrlSupportRpm = 2, + }; + + enum ChannelType{ + CHANNEL_TYPE_SERIALPORT = 0x0, + CHANNEL_TYPE_TCP = 0x1, + CHANNEL_TYPE_UDP = 0x2, + }; + + /** + * Lidar motor info + */ + struct LidarMotorInfo + { + MotorCtrlSupport motorCtrlSupport; + + // Desire speed + sl_u16 desired_speed; + + // Max speed + sl_u16 max_speed; + + // Min speed + sl_u16 min_speed; + }; + + class ILidarDriver + { + public: + virtual ~ILidarDriver() {} + + public: + /** + * Connect to LIDAR via channel + * \param channel The communication channel + * Note: you should manage the lifecycle of the channel object, make sure it is alive during lidar driver's lifecycle + */ + virtual sl_result connect(IChannel* channel) = 0; + + /** + * Disconnect from the LIDAR + */ + virtual void disconnect() = 0; + + /** + * Check if the connection is established + */ + virtual bool isConnected() = 0; + + public: + enum + { + DEFAULT_TIMEOUT = 2000 + }; + + public: + /// Ask the LIDAR core system to reset it self + /// The host system can use the Reset operation to help LIDAR escape the self-protection mode. + /// + /// \param timeout The operation timeout value (in millisecond) + virtual sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Get all scan modes that supported by lidar + virtual sl_result getAllSupportedScanModes(std::vector& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Get typical scan mode of lidar + virtual sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Start scan + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + virtual sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) = 0; + + /// Start scan in specific mode + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + virtual sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Retrieve the health status of the RPLIDAR + /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. + /// + /// \param health The health status info returned from the RPLIDAR + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. + /// + /// \param info The device information returned from the RPLIDAR + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Check whether the device support motor control + /// Note: this API will disable grab. + /// + /// \param motorCtrlSupport Return the result. + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + virtual sl_result checkMotorCtrlSupport(MotorCtrlSupport& motorCtrlSupport, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Calculate LIDAR's current scanning frequency from the given scan data + /// Please refer to the application note doc for details + /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data + /// + /// \param scanMode Lidar's current scan mode + /// \param nodes Current scan's measurements + /// \param count The number of sample nodes inside the given buffer + virtual sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) = 0; + + ///Set LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + virtual sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + ///Get LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + virtual sl_result getLidarIpConf( sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + // + /////Get LPX series lidar's MAC address + /// + /// \param macAddrArray The device MAC information returned from the LPX series lidar + /// Notice: the macAddrArray must point to a valid buffer with at least 6 bytes length + /// Otherwise, buffer overwrite will occur + virtual sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Ask the LIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Wait and grab a complete 0-360 degree scan data previously received. + /// The grabbed scan data returned by this interface always has the following charactistics: + /// + /// 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 + /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan + /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \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). + /// Once the interface returns, this parameter will store the actual received data count. + /// + /// \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. + /// + /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. + /// + /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. + virtual sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + + + /// Wait and grab a complete 0-360 degree scan data previously received with timestamp support. + /// + /// The returned timestamp belongs to the first data point of the scan data (begining of the scan). + /// Its value is represented based on the current machine's time domain with the unit of microseconds (uS). + /// + /// If the currently connected LIDAR supports hardware timestamp mechanism, this timestamp will use + /// the actual data emitted by the LIDAR device and remap it to the current machine's time domain. + /// + /// For other models that do not support hardware timestamps, this data will be deducted through estimation, + /// and there may be a slight deviation from the actual situation. + /// + /// The grabbed scan data returned by this interface always has the following charactistics: + /// + /// 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 + /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan + /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \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). + /// Once the interface returns, this parameter will store the actual received data count. + /// + /// \param timestamp_uS The reference used to store the timestamp value. + /// \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. + /// + /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. + /// + /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. + virtual sl_result grabScanDataHqWithTimeStamp(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u64 & timestamp_uS, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + + /// Ascending the scan data according to the angle value in the scan. + /// + /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData + /// + /// \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). + /// Once the interface returns, this parameter will store the actual received data count. + /// The interface will return SL_RESULT_OPERATION_FAIL when all the scan data is invalid. + virtual sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t count) = 0; + + /// Return received scan points even if it's not complete scan + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count Once the interface returns, this parameter will store the actual received data count. + /// + /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + virtual sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count) = 0; + /// Set lidar motor speed + /// The host system can use this operation to set lidar motor speed. + /// + /// \param speed The speed value set to lidar + /// + ///Note: The function will stop scan if speed is DEFAULT_MOTOR_SPEED. + virtual sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) = 0; + + /// Get the motor information of the RPLIDAR include the max speed, min speed, desired speed. + /// + /// \param motorInfo The motor information returned from the RPLIDAR + virtual sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + + /// Ask the LIDAR to use a new baudrate for serial communication + /// The target LIDAR system must support such feature to work. + /// This function does NOT check whether the target LIDAR works with the requiredBaudRate or not. + /// In order to verifiy the result, use getDeviceInfo or other getXXXX functions instead. + /// + /// \param requiredBaudRate The new baudrate required to be used. It MUST matches with the baudrate of the binded channel. + /// \param baudRateDetected The actual baudrate detected by the LIDAR system + virtual sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL) = 0; + + + + /// Get the technology of the LIDAR's measurement system + /// + /// + /// \param devInfo The device info used to deduct the result + /// If NULL is specified, a driver cached version of the connected LIDAR will be used + virtual LIDARTechnologyType getLIDARTechnologyType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0; + + + /// Get the Major Type (Series Info) of the LIDAR + /// + /// + /// \param devInfo The device info used to deduct the result + /// If NULL is specified, a driver cached version of the connected LIDAR will be used + virtual LIDARMajorType getLIDARMajorType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0; + + + /// Get the Model Name of the LIDAR + /// The result will be somthing like: "A1M8" or "S1M1" or "A3M1-R1" + /// + /// \param out_description The output string that contains the generated model name + /// + /// \param fetchAliasName If set to true, a communication will be taken to ask if there is any Alias name availabe + /// \param devInfo The device info used to deduct the result + /// If NULL is specified, a driver cached version of the connected LIDAR will be used + /// \param timeout The timeout value used by potential data communication + virtual sl_result getModelNameDescriptionString(std::string& out_description, bool fetchAliasName = true, const sl_lidar_response_device_info_t* devInfo = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + +}; + + /** + * Create a LIDAR driver instance + * + * Example + * Result channel = createSerialPortChannel("/dev/ttyUSB0", 115200); + * assert((bool)channel); + * assert(*channel); + * + * auto lidar = createLidarDriver(); + * assert((bool)lidar); + * assert(*lidar); + * + * auto res = (*lidar)->connect(*channel); + * assert(SL_IS_OK(res)); + * + * sl_lidar_response_device_info_t deviceInfo; + * res = (*lidar)->getDeviceInfo(deviceInfo); + * assert(SL_IS_OK(res)); + * + * printf("Model: %d, Firmware Version: %d.%d, Hardware Version: %d\n", + * deviceInfo.model, + * deviceInfo.firmware_version >> 8, deviceInfo.firmware_version & 0xffu, + * deviceInfo.hardware_version); + * + * delete *lidar; + * delete *channel; + */ + Result createLidarDriver(); +} diff --git a/sdk/include/sl_lidar_driver_impl.h b/sdk/include/sl_lidar_driver_impl.h new file mode 100644 index 0000000..3c1b814 --- /dev/null +++ b/sdk/include/sl_lidar_driver_impl.h @@ -0,0 +1,157 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" + +namespace sl { + class SL_LidarDriver :public ILidarDriver + { + public: + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + + enum + { + NORMAL_CAPSULE = 0, + DENSE_CAPSULE = 1, + }; + + enum { + A2A3_LIDAR_MINUM_MAJOR_ID = 2, + TOF_LIDAR_MINUM_MAJOR_ID = 6, + }; + public: + SL_LidarDriver() + :_channel(NULL) + , _isConnected(false) + , _isScanning(false) + , _isSupportingMotorCtrl(MotorCtrlSupportNone) + , _cached_sampleduration_std(LEGACY_SAMPLE_DURATION) + ,_cached_sampleduration_express(LEGACY_SAMPLE_DURATION) + , _cached_scan_node_hq_count(0) + , _cached_scan_node_hq_count_for_interval_retrieve(0) + {} + + sl_result connect(IChannel* channel); + void disconnect(); + bool isConnected(); + sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getAllSupportedScanModes(std::vector& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr); + sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT); + DEPRECATED(sl_result grabScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT)); + sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency); + sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout); + sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout); + sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs); + sl_result ascendScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t count); + sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count); + sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count); + sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_PWM);// + sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL); + + protected: + sl_result startMotor(); + sl_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result setLidarConf(sl_u32 type, const void* payload, size_t payloadSize, sl_u32 timeout); + sl_result getLidarConf(sl_u32 type, std::vector &outputBuf, const std::vector &reserve = std::vector(), sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeName(char* modeName, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + //DEPRECATED(sl_result getSampleDuration_uS(sl_lidar_response_sample_rate_t & rateInfo, sl_u32 timeout = DEFAULT_TIMEOUT)); + //DEPRECATED (sl_result checkExpressScanSupported(bool & support, sl_u32 timeout = DEFAULT_TIMEOUT)); + //DEPRECATED(sl_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)); + private: + sl_result _sendCommand(sl_u16 cmd, const void * payload = NULL, size_t payloadsize = 0 ); + sl_result _waitResponseHeader(sl_lidar_ans_header_t * header, sl_u32 timeout = DEFAULT_TIMEOUT); + template + sl_result _waitResponse(T &payload ,sl_u8 ansType, sl_u32 timeout = DEFAULT_TIMEOUT); + void _disableDataGrabbing(); + sl_result _waitNode(sl_lidar_response_measurement_node_t * node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _waitScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t & count, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheScanData(); + void _ultraCapsuleToNormal(const sl_lidar_response_ultra_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _waitCapsuledNode(sl_lidar_response_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + void _capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + void _dense_capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _cacheCapsuledScanData(); + + void _ultra_dense_capsuleToNormal(const sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& capslue, sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& nodeCount); + sl_result _waitUltraDenseCapsuledNode(sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheUltraDenseCapsuledScanData(); + + + sl_result _waitHqNode(sl_lidar_response_hq_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + void _HqToNormal(const sl_lidar_response_hq_capsule_measurement_nodes_t & node_hq, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _cacheHqScanData(); + sl_result _waitUltraCapsuledNode(sl_lidar_response_ultra_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheUltraCapsuledScanData(); + sl_result _clearRxDataCache(); + + private: + IChannel *_channel; + bool _isConnected; + bool _isScanning; + MotorCtrlSupport _isSupportingMotorCtrl; + + rp::hal::Locker _lock; + rp::hal::Event _dataEvt; + rp::hal::Thread _cachethread; + sl_u16 _cached_sampleduration_std; + sl_u16 _cached_sampleduration_express; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; + size_t _cached_scan_node_hq_count; + sl_u8 _cached_capsule_flag; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192]; + size_t _cached_scan_node_hq_count_for_interval_retrieve; + + sl_lidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + sl_lidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; + sl_lidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + sl_lidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; + bool _is_previous_capsuledataRdy; + bool _is_previous_HqdataRdy; + }; + +} diff --git a/sdk/include/sl_lidar_protocol.h b/sdk/include/sl_lidar_protocol.h new file mode 100644 index 0000000..a9a10c1 --- /dev/null +++ b/sdk/include/sl_lidar_protocol.h @@ -0,0 +1,85 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar_protocol.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + + +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable:4200) +#endif + +#include "sl_types.h" + +#define SL_LIDAR_CMD_SYNC_BYTE 0xA5 +#define SL_LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 + +#define SL_LIDAR_ANS_SYNC_BYTE1 0xA5 +#define SL_LIDAR_ANS_SYNC_BYTE2 0x5A + +#define SL_LIDAR_ANS_PKTFLAG_LOOP 0x1 + +#define SL_LIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF +#define SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) + +#if defined(_WIN32) +#pragma pack(1) +#endif + + + +typedef struct sl_lidar_cmd_packet_t +{ + sl_u8 syncByte; //must be SL_LIDAR_CMD_SYNC_BYTE + sl_u8 cmd_flag; + sl_u8 size; + sl_u8 data[0]; +} __attribute__((packed)) sl_lidar_cmd_packet_t; + + +typedef struct sl_lidar_ans_header_t +{ + sl_u8 syncByte1; // must be SL_LIDAR_ANS_SYNC_BYTE1 + sl_u8 syncByte2; // must be SL_LIDAR_ANS_SYNC_BYTE2 + sl_u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; + sl_u8 type; +} __attribute__((packed)) sl_lidar_ans_header_t; + +#if defined(_WIN32) +#pragma pack() +#endif + + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif \ No newline at end of file diff --git a/sdk/include/sl_types.h b/sdk/include/sl_types.h new file mode 100644 index 0000000..96a3755 --- /dev/null +++ b/sdk/include/sl_types.h @@ -0,0 +1,83 @@ +/* +* Slamtec LIDAR SDK +* +* sl_types.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __cplusplus +#include + +#define SL_DEFINE_TYPE(IntType, NewType) typedef std::IntType NewType +#else +#include + +#define SL_DEFINE_TYPE(IntType, NewType) typedef IntType NewType +#endif + +#define SL_DEFINE_INT_TYPE(Bits) \ + SL_DEFINE_TYPE(int ## Bits ## _t, sl_s ## Bits); \ + SL_DEFINE_TYPE(uint ## Bits ## _t, sl_u ## Bits); \ + +SL_DEFINE_INT_TYPE(8) +SL_DEFINE_INT_TYPE(16) +SL_DEFINE_INT_TYPE(32) +SL_DEFINE_INT_TYPE(64) + +#if !defined(__GNUC__) && !defined(__attribute__) +# define __attribute__(x) +#endif + +#ifdef WIN64 +typedef sl_u64 sl_word_size_t; +#elif defined(WIN32) +typedef sl_u32 sl_word_size_t; +#elif defined(__GNUC__) +typedef unsigned long sl_word_size_t; +#elif defined(__ICCARM__) +typedef sl_u32 sl_word_size_t; +#endif + +typedef uint32_t sl_result; + +#define SL_RESULT_OK (sl_result)0 +#define SL_RESULT_FAIL_BIT (sl_result)0x80000000 +#define SL_RESULT_ALREADY_DONE (sl_result)0x20 +#define SL_RESULT_INVALID_DATA (sl_result)(0x8000 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_FAIL (sl_result)(0x8001 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_TIMEOUT (sl_result)(0x8002 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_STOP (sl_result)(0x8003 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_NOT_SUPPORT (sl_result)(0x8004 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_FORMAT_NOT_SUPPORT (sl_result)(0x8005 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_INSUFFICIENT_MEMORY (sl_result)(0x8006 | SL_RESULT_FAIL_BIT) + +#define SL_IS_OK(x) ( ((x) & SL_RESULT_FAIL_BIT) == 0 ) +#define SL_IS_FAIL(x) ( ((x) & SL_RESULT_FAIL_BIT) ) diff --git a/sdk/src/arch/linux/arch_linux.h b/sdk/src/arch/linux/arch_linux.h new file mode 100644 index 0000000..ce31343 --- /dev/null +++ b/sdk/src/arch/linux/arch_linux.h @@ -0,0 +1,64 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +// libc dep +#include +#include +#include +#include +#include +#include +#include +#include + +// libc++ dep +#include +#include + +// linux specific +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "timer.h" + diff --git a/sdk/src/arch/linux/net_serial.cpp b/sdk/src/arch/linux/net_serial.cpp new file mode 100644 index 0000000..2bec556 --- /dev/null +++ b/sdk/src/arch/linux/net_serial.cpp @@ -0,0 +1,475 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/linux/arch_linux.h" +#include +#include +#include +#include +#include +// linux specific + +#include +#include + +#include +#include "hal/types.h" +#include "arch/linux/net_serial.h" +#include + +#include +//__GNUC__ +#if defined(__GNUC__) +// for Linux extension +#include +#include +#include +extern "C" int tcflush(int fildes, int queue_selector); +#else +// for other standard UNIX +#include +#include + +#endif + + +namespace rp{ namespace arch{ namespace net{ + +raw_serial::raw_serial() + : rp::hal::serial_rxtx() + , _baudrate(0) + , _flags(0) + , serial_fd(-1) +{ + _init(); +} + +raw_serial::~raw_serial() +{ + close(); + +} + +bool raw_serial::open() +{ + return open(_portName, _baudrate, _flags); +} + +bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags) +{ + strncpy(_portName, portname, sizeof(_portName)); + _baudrate = baudrate; + _flags = flags; + return true; +} + +bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) +{ + if (isOpened()) close(); + + serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY); + + if (serial_fd == -1) return false; + + + +#if !defined(__GNUC__) + // for standard UNIX + struct termios options, oldopt; + tcgetattr(serial_fd, &oldopt); + bzero(&options,sizeof(struct termios)); + + // enable rx and tx + options.c_cflag |= (CLOCAL | CREAD); + + _u32 termbaud = getTermBaudBitmap(baudrate); + + if (termbaud == (_u32)-1) { + close(); + return false; + } + cfsetispeed(&options, termbaud); + cfsetospeed(&options, termbaud); + + options.c_cflag &= ~PARENB; //no checkbit + options.c_cflag &= ~CSTOPB; //1bit stop bit + options.c_cflag &= ~CRTSCTS; //no flow control + + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; /* Select 8 data bits */ + +#ifdef CNEW_RTSCTS + options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control +#endif + + options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control + + // raw input mode + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + // raw output mode + options.c_oflag &= ~OPOST; + + + + if (tcsetattr(serial_fd, TCSANOW, &options)) + { + close(); + return false; + } + +#else + + // using Linux extension ... + struct termios2 tio; + + ioctl(serial_fd, TCGETS2, &tio); + bzero(&tio, sizeof(struct termios2)); + + tio.c_cflag = BOTHER; + tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake + + tio.c_cflag &= ~CSTOPB; //1 stop bit + tio.c_cflag &= ~CRTSCTS; //No CTS + tio.c_cflag &= ~PARENB; //No Parity + +#ifdef CNEW_RTSCTS + tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control +#endif + + tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control + + + tio.c_cc[VMIN] = 0; //min chars to read + tio.c_cc[VTIME] = 0; //time in 1/10th sec wait + + tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + // raw output mode + tio.c_oflag &= ~OPOST; + + tio.c_ispeed = baudrate; + tio.c_ospeed = baudrate; + + + ioctl(serial_fd, TCSETS2, &tio); + +#endif + + + tcflush(serial_fd, TCIFLUSH); + + if (fcntl(serial_fd, F_SETFL, FNDELAY)) + { + close(); + return false; + } + + + _is_serial_opened = true; + _operation_aborted = false; + + //Clear the DTR bit to let the motor spin + clearDTR(); + do { + // create self pipeline for wait cancellation + if (pipe(_selfpipe) == -1) break; + + int flags = fcntl(_selfpipe[0], F_GETFL); + if (flags == -1) + break; + + flags |= O_NONBLOCK; /* Make read end nonblocking */ + if (fcntl(_selfpipe[0], F_SETFL, flags) == -1) + break; + + flags = fcntl(_selfpipe[1], F_GETFL); + if (flags == -1) + break; + + flags |= O_NONBLOCK; /* Make write end nonblocking */ + if (fcntl(_selfpipe[1], F_SETFL, flags) == -1) + break; + + } while (0); + + return true; +} + +void raw_serial::close() +{ + if (serial_fd != -1) + ::close(serial_fd); + serial_fd = -1; + + if (_selfpipe[0] != -1) + ::close(_selfpipe[0]); + + if (_selfpipe[1] != -1) + ::close(_selfpipe[1]); + + _selfpipe[0] = _selfpipe[1] = -1; + + _operation_aborted = false; + _is_serial_opened = false; +} + +int raw_serial::senddata(const unsigned char * data, size_t size) +{ +// FIXME: non-block io should be used + if (!isOpened()) return 0; + + if (data == NULL || size ==0) return 0; + + size_t tx_len = 0; + required_tx_cnt = 0; + do { + int ans = ::write(serial_fd, data + tx_len, size-tx_len); + + if (ans == -1) return tx_len; + + tx_len += ans; + required_tx_cnt = tx_len; + }while (tx_len(serial_fd, _selfpipe[0]) + 1; + + /* Initialize the timeout structure */ + timeout_val.tv_sec = timeout / 1000; + timeout_val.tv_usec = (timeout % 1000) * 1000; + + if ( isOpened() ) + { + int nread; + + if ( ioctl(serial_fd, FIONREAD, &nread) == -1) return ANS_DEV_ERR; + + *returned_size = nread; + + if (*returned_size >= data_count) + { + return 0; + } + } + + while ( isOpened() ) + { + /* Do the select */ + int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val); + + if (n < 0) + { + // select error + *returned_size = 0; + return ANS_DEV_ERR; + } + else if (n == 0) + { + // time out + *returned_size =0; + return ANS_TIMEOUT; + } + else + { + if (FD_ISSET(_selfpipe[0], &input_set)) { + // require aborting the current operation + int ch; + for (;;) { + if (::read(_selfpipe[0], &ch, 1) == -1) { + break; + } + + } + + // treat as timeout + *returned_size = 0; + return ANS_TIMEOUT; + } + + // data avaliable + assert (FD_ISSET(serial_fd, &input_set)); + + + if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR; + if (*returned_size >= data_count) + { + return 0; + } + } + + } + + *returned_size=0; + return ANS_DEV_ERR; +} + +size_t raw_serial::rxqueue_count() +{ + if ( !isOpened() ) return 0; + size_t remaining; + + if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0; + return remaining; +} + +void raw_serial::setDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIS, &dtr_bit); +} + +void raw_serial::clearDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIC, &dtr_bit); +} + +void raw_serial::_init() +{ + serial_fd = -1; + _portName[0] = 0; + required_tx_cnt = required_rx_cnt = 0; + _operation_aborted = false; + _selfpipe[0] = _selfpipe[1] = -1; +} + +void raw_serial::cancelOperation() +{ + _operation_aborted = true; + if (_selfpipe[1] == -1) return; + + (int)::write(_selfpipe[1], "x", 1); +} + +_u32 raw_serial::getTermBaudBitmap(_u32 baud) +{ +#define BAUD_CONV( _baud_) case _baud_: return B##_baud_ +switch (baud) { + BAUD_CONV(1200); + BAUD_CONV(1800); + BAUD_CONV(2400); + BAUD_CONV(4800); + BAUD_CONV(9600); + BAUD_CONV(19200); + BAUD_CONV(38400); + BAUD_CONV(57600); + BAUD_CONV(115200); + BAUD_CONV(230400); + BAUD_CONV(460800); + BAUD_CONV(500000); + BAUD_CONV(576000); + BAUD_CONV(921600); + BAUD_CONV(1000000); + BAUD_CONV(1152000); + BAUD_CONV(1500000); + BAUD_CONV(2000000); + BAUD_CONV(2500000); + BAUD_CONV(3000000); + BAUD_CONV(3500000); + BAUD_CONV(4000000); + } + return -1; +} + +}}} //end rp::arch::net + +//begin rp::hal +namespace rp{ namespace hal{ + +serial_rxtx * serial_rxtx::CreateRxTx() +{ + return new rp::arch::net::raw_serial(); +} + +void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx) +{ + delete rxtx; +} + +}} //end rp::hal diff --git a/sdk/src/arch/linux/net_serial.h b/sdk/src/arch/linux/net_serial.h new file mode 100644 index 0000000..627369a --- /dev/null +++ b/sdk/src/arch/linux/net_serial.h @@ -0,0 +1,90 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/abs_rxtx.h" + +namespace rp{ namespace arch{ namespace net{ + +class raw_serial : public rp::hal::serial_rxtx +{ +public: + enum{ + SERIAL_RX_BUFFER_SIZE = 512, + SERIAL_TX_BUFFER_SIZE = 128, + }; + + raw_serial(); + virtual ~raw_serial(); + virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0); + virtual bool open(); + virtual void close(); + virtual void flush( _u32 flags); + + virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); + + virtual int senddata(const unsigned char * data, size_t size); + virtual int recvdata(unsigned char * data, size_t size); + + virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); + virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); + + virtual size_t rxqueue_count(); + + virtual void setDTR(); + virtual void clearDTR(); + + _u32 getTermBaudBitmap(_u32 baud); + + virtual void cancelOperation(); + +protected: + bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); + void _init(); + + char _portName[200]; + uint32_t _baudrate; + uint32_t _flags; + + int serial_fd; + + size_t required_tx_cnt; + size_t required_rx_cnt; + + int _selfpipe[2]; + bool _operation_aborted; +}; + +}}} diff --git a/sdk/src/arch/linux/net_socket.cpp b/sdk/src/arch/linux/net_socket.cpp new file mode 100644 index 0000000..99eb2dc --- /dev/null +++ b/sdk/src/arch/linux/net_socket.cpp @@ -0,0 +1,893 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2009 - 2013 RoboPeak Project + * + * POXIS Implementation + */ + + +#include "sdkcommon.h" +#include "../../hal/socket.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + + + +namespace rp{ namespace net { + + +static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) +{ + switch (type) { + case SocketAddress::ADDRESS_TYPE_INET: + return AF_INET; + case SocketAddress::ADDRESS_TYPE_INET6: + return AF_INET6; + case SocketAddress::ADDRESS_TYPE_UNSPEC: + return AF_UNSPEC; + + default: + assert(!"should not reach here"); + return AF_UNSPEC; + } +} + + +SocketAddress::SocketAddress() +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); +} + + + +SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_platform_data)->ss_family = AF_INET; + + setAddressFromString(addrString, type); + setPort(port); +} + +SocketAddress::SocketAddress(void * platform_data) + : _platform_data(platform_data) +{} + +SocketAddress & SocketAddress::operator = (const SocketAddress &src) +{ + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); + return *this; +} + + +SocketAddress::~SocketAddress() +{ + delete reinterpret_cast(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_platform_data)->ss_family) { + case AF_INET: + return ADDRESS_TYPE_INET; + case AF_INET6: + return ADDRESS_TYPE_INET6; + default: + assert(!"should not reach here"); + return ADDRESS_TYPE_INET; + } +} + +int SocketAddress::getPort() const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + return (int)ntohs(reinterpret_cast(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_platform_data)->sin6_port = htons((short)port); + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + +u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type) +{ + int ans = 0; + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->ss_family = AF_INET; + ans = inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_platform_data)->sin6_addr); + break; + + default: + return RESULT_INVALID_DATA; + + } + setPort(prevPort); + + return ans<=0?RESULT_INVALID_DATA:RESULT_OK; +} + + +u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const +{ + int net_family = reinterpret_cast(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, buffersize); + break; + + case AF_INET6: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin6_addr, + buffer, buffersize); + + break; + } + return ans==NULL?RESULT_OPERATION_FAIL:RESULT_OK; +} + + + +size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS, SocketAddress::address_type_t type) +{ + struct addrinfo hints; + struct addrinfo *result; + int ans; + + memset(&hints, 0, sizeof(struct addrinfo)); + hints.ai_family = _halAddrTypeToOSType(type); + hints.ai_flags = AI_PASSIVE; + + if (!performDNS) { + hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST; + + } + + ans = getaddrinfo(hostname, sevicename, &hints, &result); + + addresspool.clear(); + + if (ans != 0) { + // hostname loopup failed + return 0; + } + + + for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) { + if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) { + sockaddr_storage * storagebuffer = new sockaddr_storage; + assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen); + memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen); + addresspool.push_back(SocketAddress(storagebuffer)); + } + } + + + freeaddrinfo(result); + + return addresspool.size(); +} + + +u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + if (bufferSize < sizeof(in_addr::s_addr)) return RESULT_INSUFFICIENT_MEMORY; + + memcpy(buffer, &reinterpret_cast(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)); + + + break; + case ADDRESS_TYPE_INET6: + if (bufferSize < sizeof(in6_addr::s6_addr)) return RESULT_INSUFFICIENT_MEMORY; + memcpy(buffer, reinterpret_cast(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)); + + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + + +void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type) +{ + + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_loopback; + + } + break; + default: + return; + } + + setPort(prevPort); +} + +void SocketAddress::setBroadcastAddressIPv4() +{ + + int prevPort = getPort(); + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST); + setPort(prevPort); + +} + +void SocketAddress::setAnyAddress(SocketAddress::address_type_t type) +{ + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_ANY); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_any; + + } + break; + default: + return; + } + + setPort(prevPort); + + +} + + +}} + + + +///-------------------------------- + + +namespace rp { namespace arch { namespace net{ + +using namespace rp::net; + +class _single_thread StreamSocketImpl : public StreamSocket +{ +public: + + StreamSocketImpl(int fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) ); + + enableNoDelay(true); + this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~StreamSocketImpl() + { + close(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast( const_cast(localaddr.getPlatformData())); //donnot do this at home... + assert(addr); + + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + virtual u_result connect(const SocketAddress & pairAddress) + { + const struct sockaddr * addr = reinterpret_cast(pairAddress.getPlatformData()); + int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage)); + if (!ans) return RESULT_OK; + + + switch (errno) { + case EAFNOSUPPORT: + return RESULT_OPERATION_NOT_SUPPORT; +#if 0 + case EINPROGRESS: + return RESULT_OK; //treat async connection as good status +#endif + case ETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result listen(int backlog) + { + int ans = ::listen( _socket_fd, backlog); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual StreamSocket * accept(SocketAddress * pairAddress) + { + size_t addrsize; + addrsize = sizeof(sockaddr_storage); + int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast(const_cast(pairAddress->getPlatformData())):NULL + , (socklen_t*)&addrsize); + + if (pair_socket>=0) { + return new StreamSocketImpl(pair_socket); + } else { + return NULL; + } + } + + virtual u_result waitforIncomingConnection(_u32 timeout) + { + return waitforData(timeout); + } + + virtual u_result send(const void * buffer, size_t len) + { + size_t ans = ::send( _socket_fd, buffer, len, MSG_NOSIGNAL); + if (ans == len) { + return RESULT_OK; + } else { + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } + + } + + + virtual u_result recv(void *buf, size_t len, size_t & recv_len) + { + size_t ans = ::recv( _socket_fd, buf, len, 0); + if (ans == (size_t)-1) { + recv_len = 0; + + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + + + } else { + recv_len = ans; + return RESULT_OK; + } + } + +#if 0 + virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len) + { + size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT); + if (ans == (size_t)-1) { + recv_len = 0; + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return RESULT_OK; + } else { + return RESULT_OPERATION_FAIL; + } + + + } else { + recv_len = ans; + return RESULT_OK; + } + + } +#endif + + virtual u_result getPeerAddress(SocketAddress & peerAddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast(peerAddr.getPlatformData())); //donnot do this at home... + assert(addr); + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + + } + + virtual u_result shutdown(socket_direction_mask mask) + { + int shutdw_opt ; + + switch (mask) { + case SOCKET_DIR_RD: + shutdw_opt = SHUT_RD; + break; + case SOCKET_DIR_WR: + shutdw_opt = SHUT_WR; + break; + case SOCKET_DIR_BOTH: + default: + shutdw_opt = SHUT_RDWR; + } + + int ans = ::shutdown(_socket_fd, shutdw_opt); + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableKeepAlive(bool enable) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableNoDelay(bool enable ) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + +protected: + int _socket_fd; + + +}; + + +class _single_thread DGramSocketImpl : public DGramSocket +{ +public: + + DGramSocketImpl(int fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) ); + setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~DGramSocketImpl() + { + close(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast((localaddr.getPlatformData()))); //donnot do this at home... + assert(addr); + + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len) + { + const struct sockaddr * addr = target ? reinterpret_cast(target->getPlatformData()) : NULL; + int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0); + int ans = ::sendto(_socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size); + if (ans != -1) { + assert(ans == len); + return RESULT_OK; + } else { + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + + case EMSGSIZE: + return RESULT_INVALID_DATA; + default: + return RESULT_OPERATION_FAIL; + } + + } + + } + + virtual u_result setPairAddress(const SocketAddress* pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr* addr = pairAddress ? reinterpret_cast(pairAddress->getPlatformData()) : reinterpret_cast(&unspecAddr); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + return ans ? RESULT_OPERATION_FAIL : RESULT_OK; + + } + + virtual u_result clearRxCache() + { + timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + int res = -1; + char recv_data[2]; + memset(recv_data, 0, sizeof(recv_data)); + while (true) { + res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv); + if (res == 0) break; + recv(_socket_fd, recv_data, 1, 0); + } + return RESULT_OK; + } + + virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size); + if (ans == (size_t)-1) { + recv_len = 0; + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + } else { + recv_len = ans; + return RESULT_OK; + } + + } + +#if 0 + virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + + size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size); + + if (ans == (size_t)-1) { + recv_len = 0; + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return RESULT_OK; + } else { + return RESULT_OPERATION_FAIL; + } + + + } else { + recv_len = ans; + return RESULT_OK; + } + + } +#endif + +protected: + int _socket_fd; + +}; + + +}}} + + +namespace rp { namespace net{ + + +static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family) +{ + switch (family) { + case SocketBase::SOCKET_FAMILY_INET: + return AF_INET; + case SocketBase::SOCKET_FAMILY_INET6: + return AF_INET6; + case SocketBase::SOCKET_FAMILY_RAW: + return AF_PACKET; + default: + assert(!"should not reach here"); + return AF_INET; // force treating as IPv4 in release mode + } + +} + +StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family) +{ + if (family == SOCKET_FAMILY_RAW) return NULL; + + + int socket_family = _socketHalFamilyToOSFamily(family); + int socket_fd = ::socket(socket_family, SOCK_STREAM, 0); + if (socket_fd == -1) return NULL; + + StreamSocket * newborn = static_cast(new rp::arch::net::StreamSocketImpl(socket_fd)); + return newborn; + +} + + +DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family) +{ + int socket_family = _socketHalFamilyToOSFamily(family); + + + int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0); + if (socket_fd == -1) return NULL; + + DGramSocket * newborn = static_cast(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/sdk/src/arch/linux/thread.hpp b/sdk/src/arch/linux/thread.hpp new file mode 100644 index 0000000..6a9107b --- /dev/null +++ b/sdk/src/arch/linux/thread.hpp @@ -0,0 +1,185 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/linux/arch_linux.h" + +#include +#include +#include +#include +#include + +namespace rp{ namespace hal{ + +Thread Thread::create(thread_proc_t proc, void * data) +{ + Thread newborn(proc, data); + + // tricky code, we assume pthread_t is not a structure but a word size value + assert( sizeof(newborn._handle) >= sizeof(pthread_t)); + + pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data); + + return newborn; +} + +u_result Thread::terminate() +{ + if (!this->_handle) return RESULT_OK; + + return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; +} + +u_result Thread::SetSelfPriority( priority_val_t p) +{ + + pid_t selfTid = syscall(SYS_gettid); + + // check whether current schedule policy supports priority levels + int current_policy = SCHED_OTHER; + struct sched_param current_param; + int nice = 0; + int ans; + + if (sched_getparam(selfTid, ¤t_param)) + { + // cannot retreieve values + return RESULT_OPERATION_FAIL; + } + + int pthread_priority_min; + +#if 1 + pthread_priority_min = sched_get_priority_min(SCHED_RR); +#else + pthread_priority_min = 1; +#endif + int pthread_priority = 0 ; + + switch(p) + { + case PRIORITY_REALTIME: + //pthread_priority = pthread_priority_max; + current_policy = SCHED_RR; + pthread_priority = pthread_priority_min + 1; + nice = 0; + break; + case PRIORITY_HIGH: + //pthread_priority = (pthread_priority_max + pthread_priority_min)/2; + current_policy = SCHED_RR; + pthread_priority = pthread_priority_min; + nice = 0; + break; + case PRIORITY_NORMAL: + pthread_priority = 0; + current_policy = SCHED_OTHER; + nice = 0; + break; + case PRIORITY_LOW: + pthread_priority = 0; + current_policy = SCHED_OTHER; + nice = 10; + break; + case PRIORITY_IDLE: + pthread_priority = 0; + current_policy = SCHED_IDLE; + nice = 0; + break; + } + // change the inhertiable behavior + current_policy |= SCHED_RESET_ON_FORK; + + current_param.__sched_priority = pthread_priority; + + + + + // do not use pthread version as it will make the priority be inherited by a thread child + if ( (ans = sched_setscheduler(selfTid, current_policy , ¤t_param)) ) + { + if (ans == EPERM) + { + //DBG_PRINT("warning, current process hasn't the right permission to set threads priority\n"); + } + return RESULT_OPERATION_FAIL; + } + + + if ((current_policy == SCHED_OTHER) || (current_policy == SCHED_BATCH)) + { + if (setpriority(PRIO_PROCESS, selfTid, nice)) { + return RESULT_OPERATION_FAIL; + } + } + + + return RESULT_OK; +} + +Thread::priority_val_t Thread::getPriority() +{ + if (!this->_handle) return PRIORITY_NORMAL; + + int current_policy; + struct sched_param current_param; + if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) + { + // cannot retreieve values + return PRIORITY_NORMAL; + } + + int pthread_priority_max = sched_get_priority_max(SCHED_RR); + int pthread_priority_min = sched_get_priority_min(SCHED_RR); + + if (current_param.__sched_priority ==(pthread_priority_max )) + { + return PRIORITY_REALTIME; + } + if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2) + { + return PRIORITY_HIGH; + } + return PRIORITY_NORMAL; +} + +u_result Thread::join(unsigned long timeout) +{ + if (!this->_handle) return RESULT_OK; + + pthread_join((pthread_t)(this->_handle), NULL); + this->_handle = 0; + return RESULT_OK; +} + +}} diff --git a/sdk/src/arch/linux/timer.cpp b/sdk/src/arch/linux/timer.cpp new file mode 100644 index 0000000..a727e46 --- /dev/null +++ b/sdk/src/arch/linux/timer.cpp @@ -0,0 +1,52 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/linux/arch_linux.h" + +namespace rp{ namespace arch{ +_u64 rp_getus() +{ + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return t.tv_sec*1000000LL + t.tv_nsec/1000; +} +_u64 rp_getms() +{ + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return t.tv_sec*1000L + t.tv_nsec/1000000L; +} +}} diff --git a/sdk/src/arch/linux/timer.h b/sdk/src/arch/linux/timer.h new file mode 100644 index 0000000..1641001 --- /dev/null +++ b/sdk/src/arch/linux/timer.h @@ -0,0 +1,59 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/types.h" + +#include +static inline void delay(_word_size_t ms){ + while (ms>=1000){ + usleep(1000*1000); + ms-=1000; + }; + if (ms!=0) + usleep(ms*1000); +} + +// TODO: the highest timer interface should be clock_gettime +namespace rp{ namespace arch{ + +_u64 rp_getus(); +_u64 rp_getms(); + +}} + +#define getms() rp::arch::rp_getms() +#define getus() rp::arch::rp_getus() + diff --git a/sdk/src/arch/macOS/arch_macOS.h b/sdk/src/arch/macOS/arch_macOS.h new file mode 100644 index 0000000..950ab97 --- /dev/null +++ b/sdk/src/arch/macOS/arch_macOS.h @@ -0,0 +1,66 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +// libc dep +#include +#include +#include +#include +#include +#include +#include +#include + +// libc++ dep +#include +#include + + +// POSIX specific +extern "C" { +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +#include "arch/macOS/timer.h" + diff --git a/sdk/src/arch/macOS/net_serial.cpp b/sdk/src/arch/macOS/net_serial.cpp new file mode 100644 index 0000000..d1859fa --- /dev/null +++ b/sdk/src/arch/macOS/net_serial.cpp @@ -0,0 +1,346 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/macOS/arch_macOS.h" +#include "arch/macOS/net_serial.h" +#include +#include +#include + +namespace rp{ namespace arch{ namespace net{ + +raw_serial::raw_serial() + : rp::hal::serial_rxtx() + , _baudrate(0) + , _flags(0) + , serial_fd(-1) +{ + _init(); +} + +raw_serial::~raw_serial() +{ + close(); + +} + +bool raw_serial::open() +{ + return open(_portName, _baudrate, _flags); +} + +bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags) +{ + strncpy(_portName, portname, sizeof(_portName)); + _baudrate = baudrate; + _flags = flags; + return true; +} + +bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) +{ + if (isOpened()) close(); + + serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY); + + if (serial_fd == -1) return false; + + struct termios options, oldopt; + tcgetattr(serial_fd, &oldopt); + bzero(&options,sizeof(struct termios)); + + cfsetspeed(&options, B19200); + + // enable rx and tx + options.c_cflag |= (CLOCAL | CREAD); + + + options.c_cflag &= ~PARENB; //no checkbit + options.c_cflag &= ~CSTOPB; //1bit stop bit + + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; /* Select 8 data bits */ + +#ifdef CNEW_RTSCTS + options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control +#endif + + options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control + + // raw input mode + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + // raw output mode + options.c_oflag &= ~OPOST; + + tcflush(serial_fd,TCIFLUSH); + + if (tcsetattr(serial_fd, TCSANOW, &options)) + { + close(); + return false; + } + + printf("Setting serial port baudrate...\n"); + + speed_t speed = (speed_t)baudrate; + if (ioctl(serial_fd, IOSSIOSPEED, &speed)== -1) { + printf("Error calling ioctl(..., IOSSIOSPEED, ...) %s - %s(%d).\n", + portname, strerror(errno), errno); + close(); + return false; + } + + _is_serial_opened = true; + + //Clear the DTR bit to let the motor spin + clearDTR(); + + return true; +} + +void raw_serial::close() +{ + if (serial_fd != -1) + ::close(serial_fd); + serial_fd = -1; + + _is_serial_opened = false; +} + +int raw_serial::senddata(const unsigned char * data, size_t size) +{ +// FIXME: non-block io should be used + if (!isOpened()) return 0; + + if (data == NULL || size ==0) return 0; + + size_t tx_len = 0; + required_tx_cnt = 0; + do { + int ans = ::write(serial_fd, data + tx_len, size-tx_len); + + if (ans == -1) return tx_len; + + tx_len += ans; + required_tx_cnt = tx_len; + }while (tx_len= data_count) + { + return 0; + } + } + + while ( isOpened() ) + { + /* Do the select */ + int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val); + + if (n < 0) + { + // select error + *returned_size = 0; + return ANS_DEV_ERR; + } + else if (n == 0) + { + // time out + *returned_size =0; + return ANS_TIMEOUT; + } + else + { + // data avaliable + assert (FD_ISSET(serial_fd, &input_set)); + + + if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR; + if (*returned_size >= data_count) + { + return 0; + } + } + + } + + *returned_size=0; + return ANS_DEV_ERR; +} + +size_t raw_serial::rxqueue_count() +{ + if ( !isOpened() ) return 0; + size_t remaining; + + if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0; + return remaining; +} + +void raw_serial::setDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIS, &dtr_bit); +} + +void raw_serial::clearDTR() +{ + if ( !isOpened() ) return; + + uint32_t dtr_bit = TIOCM_DTR; + ioctl(serial_fd, TIOCMBIC, &dtr_bit); +} + +void raw_serial::_init() +{ + serial_fd = 0; + _portName[0] = 0; + required_tx_cnt = required_rx_cnt = 0; +} + + + +_u32 raw_serial::getTermBaudBitmap(_u32 baud) +{ +#define BAUD_CONV(_baud_) case _baud_: return _baud_ + switch (baud) + { + BAUD_CONV(1200); + BAUD_CONV(1800); + BAUD_CONV(2400); + BAUD_CONV(4800); + BAUD_CONV(9600); + BAUD_CONV(19200); + BAUD_CONV(38400); + BAUD_CONV(57600); + BAUD_CONV(115200); + BAUD_CONV(230400); + BAUD_CONV(460800); + BAUD_CONV(500000); + BAUD_CONV(576000); + BAUD_CONV(921600); + BAUD_CONV(1000000); + BAUD_CONV(1152000); + BAUD_CONV(1500000); + BAUD_CONV(2000000); + BAUD_CONV(2500000); + BAUD_CONV(3000000); + BAUD_CONV(3500000); + BAUD_CONV(4000000); + } + return -1; +} + +}}} //end rp::arch::net + + + +//begin rp::hal +namespace rp{ namespace hal{ + + serial_rxtx * serial_rxtx::CreateRxTx() + { + return new rp::arch::net::raw_serial(); + } + + void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx) + { + delete rxtx; + } + +}} //end rp::hal diff --git a/sdk/src/arch/macOS/net_serial.h b/sdk/src/arch/macOS/net_serial.h new file mode 100644 index 0000000..41db813 --- /dev/null +++ b/sdk/src/arch/macOS/net_serial.h @@ -0,0 +1,84 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/abs_rxtx.h" + +namespace rp{ namespace arch{ namespace net{ + +class raw_serial : public rp::hal::serial_rxtx +{ +public: + enum{ + SERIAL_RX_BUFFER_SIZE = 512, + SERIAL_TX_BUFFER_SIZE = 128, + }; + + raw_serial(); + virtual ~raw_serial(); + virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0); + virtual bool open(); + virtual void close(); + virtual void flush( _u32 flags); + + virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); + + virtual int senddata(const unsigned char * data, size_t size); + virtual int recvdata(unsigned char * data, size_t size); + + virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); + virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); + + virtual size_t rxqueue_count(); + + virtual void setDTR(); + virtual void clearDTR(); + + _u32 getTermBaudBitmap(_u32 baud); +protected: + bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); + void _init(); + + char _portName[200]; + uint32_t _baudrate; + uint32_t _flags; + + int serial_fd; + + size_t required_tx_cnt; + size_t required_rx_cnt; +}; + +}}} diff --git a/sdk/src/arch/macOS/net_socket.cpp b/sdk/src/arch/macOS/net_socket.cpp new file mode 100644 index 0000000..2208fac --- /dev/null +++ b/sdk/src/arch/macOS/net_socket.cpp @@ -0,0 +1,899 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2018 RoboPeak Project + * + * macOS Implementation + */ + + +#include "sdkcommon.h" +#include "../../hal/socket.h" + +#include +#include +#include +#include +#include + +namespace rp{ namespace net { + + +static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) +{ + switch (type) { + case SocketAddress::ADDRESS_TYPE_INET: + return AF_INET; + case SocketAddress::ADDRESS_TYPE_INET6: + return AF_INET6; + case SocketAddress::ADDRESS_TYPE_UNSPEC: + return AF_UNSPEC; + + default: + assert(!"should not reach here"); + return AF_UNSPEC; + } +} + + +SocketAddress::SocketAddress() +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); +} + + + +SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_platform_data)->ss_family = AF_INET; + + setAddressFromString(addrString, type); + setPort(port); +} + +SocketAddress::SocketAddress(void * platform_data) + : _platform_data(platform_data) +{} + +SocketAddress & SocketAddress::operator = (const SocketAddress &src) +{ + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); + return *this; +} + + +SocketAddress::~SocketAddress() +{ + delete reinterpret_cast(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_platform_data)->ss_family) { + case AF_INET: + return ADDRESS_TYPE_INET; + case AF_INET6: + return ADDRESS_TYPE_INET6; + default: + assert(!"should not reach here"); + return ADDRESS_TYPE_INET; + } +} + +int SocketAddress::getPort() const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + return (int)ntohs(reinterpret_cast(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_platform_data)->sin6_port = htons((short)port); + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + +u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type) +{ + int ans = 0; + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->ss_family = AF_INET; + ans = inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_platform_data)->sin6_addr); + break; + + default: + return RESULT_INVALID_DATA; + + } + setPort(prevPort); + + return ans<=0?RESULT_INVALID_DATA:RESULT_OK; +} + + +u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const +{ + int net_family = reinterpret_cast(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, buffersize); + break; + + case AF_INET6: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin6_addr, + buffer, buffersize); + + break; + } + return !ans?RESULT_OPERATION_FAIL:RESULT_OK; +} + + + +size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS, SocketAddress::address_type_t type) +{ + struct addrinfo hints; + struct addrinfo *result; + int ans; + + memset(&hints, 0, sizeof(struct addrinfo)); + hints.ai_family = _halAddrTypeToOSType(type); + hints.ai_flags = AI_PASSIVE; + + if (!performDNS) { + hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST; + + } + + ans = getaddrinfo(hostname, sevicename, &hints, &result); + + addresspool.clear(); + + if (ans != 0) { + // hostname loopup failed + return 0; + } + + + for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) { + if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) { + sockaddr_storage * storagebuffer = new sockaddr_storage; + assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen); + memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen); + addresspool.push_back(SocketAddress(storagebuffer)); + } + } + + + freeaddrinfo(result); + + return addresspool.size(); +} + + +u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + if (bufferSize < sizeof(in_addr_t)) return RESULT_INSUFFICIENT_MEMORY; + + memcpy(buffer, &reinterpret_cast(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)); + + + break; + case ADDRESS_TYPE_INET6: + if (bufferSize < sizeof(in6_addr)) return RESULT_INSUFFICIENT_MEMORY; + memcpy(buffer, reinterpret_cast(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)); + + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + + +void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type) +{ + + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_loopback; + + } + break; + default: + return; + } + + setPort(prevPort); +} + +void SocketAddress::setBroadcastAddressIPv4() +{ + + int prevPort = getPort(); + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST); + setPort(prevPort); + +} + +void SocketAddress::setAnyAddress(SocketAddress::address_type_t type) +{ + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_ANY); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_any; + + } + break; + default: + return; + } + + setPort(prevPort); + + +} + + +}} + + + +///-------------------------------- + + +namespace rp { namespace arch { namespace net{ + +using namespace rp::net; + +class _single_thread StreamSocketImpl : public StreamSocket +{ +public: + + StreamSocketImpl(int fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) ); + ::setsockopt( _socket_fd, SOL_SOCKET, SO_NOSIGPIPE, (char*)&bool_true, sizeof(bool_true)); + + enableNoDelay(true); + this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~StreamSocketImpl() + { + close(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast( const_cast(localaddr.getPlatformData())); //donnot do this at home... + assert(addr); + + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + virtual u_result connect(const SocketAddress & pairAddress) + { + const struct sockaddr * addr = reinterpret_cast(pairAddress.getPlatformData()); + + int ans; + if (pairAddress.getAddressType() == SocketAddress::ADDRESS_TYPE_INET) { + ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in)); + } else { + ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in6)); + } + + if (!ans) return RESULT_OK; + + + switch (errno) { + case EAFNOSUPPORT: + return RESULT_OPERATION_NOT_SUPPORT; +#if 0 + case EINPROGRESS: + return RESULT_OK; //treat async connection as good status +#endif + case ETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result listen(int backlog) + { + int ans = ::listen( _socket_fd, backlog); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual StreamSocket * accept(SocketAddress * pairAddress) + { + size_t addrsize; + addrsize = sizeof(sockaddr_storage); + int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast(const_cast(pairAddress->getPlatformData())):NULL + , (socklen_t*)&addrsize); + + if (pair_socket>=0) { + return new StreamSocketImpl(pair_socket); + } else { + return NULL; + } + } + + virtual u_result waitforIncomingConnection(_u32 timeout) + { + return waitforData(timeout); + } + + virtual u_result send(const void * buffer, size_t len) + { + size_t ans = ::send( _socket_fd, buffer, len, 0); + if (ans == (int)len) { + return RESULT_OK; + } else { + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } + + } + + + virtual u_result recv(void *buf, size_t len, size_t & recv_len) + { + size_t ans = ::recv( _socket_fd, buf, len, 0); + if (ans == (size_t)-1) { + recv_len = 0; + + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + + + } else { + recv_len = ans; + return RESULT_OK; + } + } + +#if 0 + virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len) + { + size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT); + if (ans == (size_t)-1) { + recv_len = 0; + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return RESULT_OK; + } else { + return RESULT_OPERATION_FAIL; + } + + + } else { + recv_len = ans; + return RESULT_OK; + } + + } +#endif + + virtual u_result getPeerAddress(SocketAddress & peerAddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast(peerAddr.getPlatformData())); //donnot do this at home... + assert(addr); + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + + } + + virtual u_result shutdown(socket_direction_mask mask) + { + int shutdw_opt ; + + switch (mask) { + case SOCKET_DIR_RD: + shutdw_opt = SHUT_RD; + break; + case SOCKET_DIR_WR: + shutdw_opt = SHUT_WR; + break; + case SOCKET_DIR_BOTH: + default: + shutdw_opt = SHUT_RDWR; + } + + int ans = ::shutdown(_socket_fd, shutdw_opt); + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableKeepAlive(bool enable) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableNoDelay(bool enable ) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + +protected: + int _socket_fd; + + +}; + + +class _single_thread DGramSocketImpl : public DGramSocket +{ +public: + + DGramSocketImpl(int fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) ); + setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~DGramSocketImpl() + { + close(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast((localaddr.getPlatformData()))); //donnot do this at home... + assert(addr); + + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len) + { + const struct sockaddr * addr = target ? reinterpret_cast(target->getPlatformData()) : NULL; + int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0); + int ans = ::sendto(_socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size); + if (ans != -1) { + assert(ans == (int)len); + return RESULT_OK; + } else { + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + + case EMSGSIZE: + return RESULT_INVALID_DATA; + default: + return RESULT_OPERATION_FAIL; + } + + } + + } + virtual u_result setPairAddress(const SocketAddress* pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr* addr = pairAddress ? reinterpret_cast(pairAddress->getPlatformData()) : reinterpret_cast(&unspecAddr); + int ans; + if (pairAddress->getAddressType() == SocketAddress::ADDRESS_TYPE_INET) { + ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in)); + } else { + ans = ::connect(_socket_fd, addr, sizeof(sockaddr_in6)); + } + return ans ? RESULT_OPERATION_FAIL : RESULT_OK; + + } + + virtual u_result clearRxCache() + { + timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + int res = -1; + char recv_data[2]; + memset(recv_data, 0, sizeof(recv_data)); + while (true) { + res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv); + if (res == 0) break; + recv(_socket_fd, recv_data, 1, 0); + } + return RESULT_OK; + } + + virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size); + if (ans == (size_t)-1) { + recv_len = 0; + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + } else { + recv_len = ans; + return RESULT_OK; + } + + } + +#if 0 + virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + + size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size); + + if (ans == (size_t)-1) { + recv_len = 0; + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return RESULT_OK; + } else { + return RESULT_OPERATION_FAIL; + } + + + } else { + recv_len = ans; + return RESULT_OK; + } + + } +#endif + +protected: + int _socket_fd; + +}; + + +}}} + + +namespace rp { namespace net{ + + +static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family) +{ + switch (family) { + case SocketBase::SOCKET_FAMILY_INET: + return AF_INET; + case SocketBase::SOCKET_FAMILY_INET6: + return AF_INET6; + case SocketBase::SOCKET_FAMILY_RAW: + assert(!"should not reach here, AF_PACKET is not supported on macOS"); + return AF_INET; + default: + assert(!"should not reach here"); + return AF_INET; // force treating as IPv4 in release mode + } + +} + +StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family) +{ + if (family == SOCKET_FAMILY_RAW) return NULL; + + + int socket_family = _socketHalFamilyToOSFamily(family); + int socket_fd = ::socket(socket_family, SOCK_STREAM, 0); + if (socket_fd == -1) return NULL; + + StreamSocket * newborn = static_cast(new rp::arch::net::StreamSocketImpl(socket_fd)); + return newborn; + +} + + +DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family) +{ + int socket_family = _socketHalFamilyToOSFamily(family); + + + int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0); + if (socket_fd == -1) return NULL; + + DGramSocket * newborn = static_cast(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/sdk/src/arch/macOS/thread.hpp b/sdk/src/arch/macOS/thread.hpp new file mode 100644 index 0000000..72fd8f8 --- /dev/null +++ b/sdk/src/arch/macOS/thread.hpp @@ -0,0 +1,79 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/macOS/arch_macOS.h" + +namespace rp{ namespace hal{ + +Thread Thread::create(thread_proc_t proc, void * data) +{ + Thread newborn(proc, data); + + // tricky code, we assume pthread_t is not a structure but a word size value + assert( sizeof(newborn._handle) >= sizeof(pthread_t)); + + pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data); + + return newborn; +} + +u_result Thread::terminate() +{ + if (!this->_handle) return RESULT_OK; + + // return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; + return RESULT_OK; +} + +u_result Thread::SetSelfPriority( priority_val_t p) +{ + // simply ignore this request + return RESULT_OK; +} + +Thread::priority_val_t Thread::getPriority() +{ + return PRIORITY_NORMAL; +} + +u_result Thread::join(unsigned long timeout) +{ + if (!this->_handle) return RESULT_OK; + + pthread_join((pthread_t)(this->_handle), NULL); + this->_handle = 0; + return RESULT_OK; +} + +}} diff --git a/sdk/src/arch/macOS/timer.cpp b/sdk/src/arch/macOS/timer.cpp new file mode 100644 index 0000000..cae6e57 --- /dev/null +++ b/sdk/src/arch/macOS/timer.cpp @@ -0,0 +1,54 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "arch/macOS/arch_macOS.h" + + +namespace rp{ namespace arch{ +_u64 rp_getus() +{ + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return t.tv_sec*1000000LL + t.tv_nsec/1000; +} +_u64 rp_getms() +{ + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return t.tv_sec*1000L + t.tv_nsec/1000000L; +} + +}} diff --git a/sdk/src/arch/macOS/timer.h b/sdk/src/arch/macOS/timer.h new file mode 100644 index 0000000..8f5ef04 --- /dev/null +++ b/sdk/src/arch/macOS/timer.h @@ -0,0 +1,58 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "rptypes.h" + +#include +static inline void delay(_word_size_t ms){ + while (ms>=1000){ + usleep(1000*1000); + ms-=1000; + }; + if (ms!=0) + usleep(ms*1000); +} + +// TODO: the highest timer interface should be clock_gettime +namespace rp{ namespace arch{ + +_u64 rp_getus(); +_u64 rp_getms(); + +}} + +#define getms() rp::arch::rp_getms() +#define getus() rp::arch::rp_getus() diff --git a/sdk/src/arch/win32/arch_win32.h b/sdk/src/arch/win32/arch_win32.h new file mode 100644 index 0000000..3ae6655 --- /dev/null +++ b/sdk/src/arch/win32/arch_win32.h @@ -0,0 +1,66 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#pragma warning (disable: 4996) +#define _CRT_SECURE_NO_WARNINGS + +#ifndef WINVER +#define WINVER 0x0500 +#endif + +#ifndef _WIN32_WINNT +#define _WIN32_WINNT 0x0501 +#endif + + +#ifndef _WIN32_IE +#define _WIN32_IE 0x0501 +#endif + +#ifndef _RICHEDIT_VER +#define _RICHEDIT_VER 0x0200 +#endif + + +#include +#include +#include +#include //for memcpy etc.. +#include +#include + + +#include "timer.h" diff --git a/sdk/src/arch/win32/net_serial.cpp b/sdk/src/arch/win32/net_serial.cpp new file mode 100644 index 0000000..f1fe025 --- /dev/null +++ b/sdk/src/arch/win32/net_serial.cpp @@ -0,0 +1,367 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "net_serial.h" + +namespace rp{ namespace arch{ namespace net{ + +raw_serial::raw_serial() + : rp::hal::serial_rxtx() + , _serial_handle(NULL) + , _baudrate(0) + , _flags(0) +{ + _init(); +} + +raw_serial::~raw_serial() +{ + close(); + + CloseHandle(_ro.hEvent); + CloseHandle(_wo.hEvent); + CloseHandle(_wait_o.hEvent); +} + +bool raw_serial::open() +{ + return open(_portName, _baudrate, _flags); +} + +bool raw_serial::bind(const char * portname, _u32 baudrate, _u32 flags) +{ + strncpy(_portName, portname, sizeof(_portName)); + _baudrate = baudrate; + _flags = flags; + return true; +} + +bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags) +{ +#ifdef _UNICODE + wchar_t wportname[1024]; + mbstowcs(wportname, portname, sizeof(wportname) / sizeof(wchar_t)); +#endif + + if (isOpened()) close(); + + _serial_handle = CreateFile( +#ifdef _UNICODE + wportname, +#else + portname, +#endif + GENERIC_READ | GENERIC_WRITE, + 0, + NULL, + OPEN_EXISTING, + FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, + NULL + ); + + if (_serial_handle == INVALID_HANDLE_VALUE) return false; + + if (!SetupComm(_serial_handle, SERIAL_RX_BUFFER_SIZE, SERIAL_TX_BUFFER_SIZE)) + { + close(); + return false; + } + + _dcb.BaudRate = baudrate; + _dcb.ByteSize = 8; + _dcb.Parity = NOPARITY; + _dcb.StopBits = ONESTOPBIT; + _dcb.fDtrControl = DTR_CONTROL_ENABLE; + + if (!SetCommState(_serial_handle, &_dcb)) + { + close(); + return false; + } + + if (!SetCommTimeouts(_serial_handle, &_co)) + { + close(); + return false; + } + + if (!SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR )) + { + close(); + return false; + } + + if (!PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR )) + { + close(); + return false; + } + + Sleep(30); + _is_serial_opened = true; + + //Clear the DTR bit set DTR=high + clearDTR(); + + return true; +} + +void raw_serial::close() +{ + SetCommMask(_serial_handle, 0); + ResetEvent(_wait_o.hEvent); + + CloseHandle(_serial_handle); + _serial_handle = INVALID_HANDLE_VALUE; + + _is_serial_opened = false; +} + +int raw_serial::senddata(const unsigned char * data, size_t size) +{ + DWORD error; + DWORD w_len = 0, o_len = -1; + if (!isOpened()) return ANS_DEV_ERR; + + if (data == NULL || size ==0) return 0; + + if(ClearCommError(_serial_handle, &error, NULL) && error > 0) + PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_TXCLEAR); + + if(!WriteFile(_serial_handle, data, (DWORD)size, &w_len, &_wo)) + if(GetLastError() != ERROR_IO_PENDING) + w_len = ANS_DEV_ERR; + + return w_len; +} + +int raw_serial::recvdata(unsigned char * data, size_t size) +{ + if (!isOpened()) return 0; + DWORD r_len = 0; + + + if(!ReadFile(_serial_handle, data, (DWORD)size, &r_len, &_ro)) + { + if(GetLastError() == ERROR_IO_PENDING) + { + if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) + { + if(GetLastError() != ERROR_IO_INCOMPLETE) + r_len = 0; + } + } + else + r_len = 0; + } + + return r_len; +} + +void raw_serial::flush( _u32 flags) +{ + PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR ); +} + +int raw_serial::waitforsent(_u32 timeout, size_t * returned_size) +{ + if (!isOpened() ) return ANS_DEV_ERR; + DWORD w_len = 0; + _word_size_t ans =0; + + if (WaitForSingleObject(_wo.hEvent, timeout) == WAIT_TIMEOUT) + { + ans = ANS_TIMEOUT; + goto _final; + } + if(!GetOverlappedResult(_serial_handle, &_wo, &w_len, FALSE)) + { + ans = ANS_DEV_ERR; + } +_final: + if (returned_size) *returned_size = w_len; + return (int)ans; +} + +int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size) +{ + if (!isOpened() ) return -1; + DWORD r_len = 0; + _word_size_t ans =0; + + if (WaitForSingleObject(_ro.hEvent, timeout) == WAIT_TIMEOUT) + { + ans = ANS_TIMEOUT; + } + if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) + { + ans = ANS_DEV_ERR; + } + if (returned_size) *returned_size = r_len; + return (int)ans; +} + +int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size) +{ + COMSTAT stat; + DWORD error; + DWORD msk,length; + size_t dummy_length; + + if (returned_size==NULL) returned_size=(size_t *)&dummy_length; + + + if ( isOpened()) { + size_t rxqueue_remaining = rxqueue_count(); + if (rxqueue_remaining >= data_count) { + *returned_size = rxqueue_remaining; + return 0; + } + } + + while ( isOpened() ) + { + msk = 0; + SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR ); + if(!WaitCommEvent(_serial_handle, &msk, &_wait_o)) + { + if(GetLastError() == ERROR_IO_PENDING) + { + if (WaitForSingleObject(_wait_o.hEvent, timeout) == WAIT_TIMEOUT) + { + *returned_size =0; + return ANS_TIMEOUT; + } + + GetOverlappedResult(_serial_handle, &_wait_o, &length, TRUE); + + ::ResetEvent(_wait_o.hEvent); + }else + { + ClearCommError(_serial_handle, &error, &stat); + *returned_size = stat.cbInQue; + return ANS_DEV_ERR; + } + } + + if(msk & EV_ERR){ + // FIXME: may cause problem here + ClearCommError(_serial_handle, &error, &stat); + } + + if(msk & EV_RXCHAR){ + ClearCommError(_serial_handle, &error, &stat); + if(stat.cbInQue >= data_count) + { + *returned_size = stat.cbInQue; + return 0; + } + } + } + *returned_size=0; + return ANS_DEV_ERR; +} + +size_t raw_serial::rxqueue_count() +{ + if ( !isOpened() ) return 0; + COMSTAT com_stat; + DWORD error; + DWORD r_len = 0; + + if(ClearCommError(_serial_handle, &error, &com_stat) && error > 0) + { + PurgeComm(_serial_handle, PURGE_RXABORT | PURGE_RXCLEAR); + return 0; + } + return com_stat.cbInQue; +} + +void raw_serial::setDTR() +{ + if ( !isOpened() ) return; + + EscapeCommFunction(_serial_handle, SETDTR); +} + +void raw_serial::clearDTR() +{ + if ( !isOpened() ) return; + + EscapeCommFunction(_serial_handle, CLRDTR); +} + + +void raw_serial::_init() +{ + memset(&_dcb, 0, sizeof(_dcb)); + _dcb.DCBlength = sizeof(_dcb); + _serial_handle = INVALID_HANDLE_VALUE; + memset(&_co, 0, sizeof(_co)); + _co.ReadIntervalTimeout = 0; + _co.ReadTotalTimeoutMultiplier = 0; + _co.ReadTotalTimeoutConstant = 0; + _co.WriteTotalTimeoutMultiplier = 0; + _co.WriteTotalTimeoutConstant = 0; + + memset(&_ro, 0, sizeof(_ro)); + memset(&_wo, 0, sizeof(_wo)); + memset(&_wait_o, 0, sizeof(_wait_o)); + + _ro.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + _wo.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + _wait_o.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + + _portName[0] = 0; +} + +}}} //end rp::arch::net + + +//begin rp::hal +namespace rp{ namespace hal{ + +serial_rxtx * serial_rxtx::CreateRxTx() +{ + return new rp::arch::net::raw_serial(); +} + +void serial_rxtx::ReleaseRxTx( serial_rxtx * rxtx) +{ + delete rxtx; +} + + +}} //end rp::hal diff --git a/sdk/src/arch/win32/net_serial.h b/sdk/src/arch/win32/net_serial.h new file mode 100644 index 0000000..d43137e --- /dev/null +++ b/sdk/src/arch/win32/net_serial.h @@ -0,0 +1,86 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/abs_rxtx.h" + +namespace rp{ namespace arch{ namespace net{ + +class raw_serial : public rp::hal::serial_rxtx +{ +public: + enum{ + SERIAL_RX_BUFFER_SIZE = 512, + SERIAL_TX_BUFFER_SIZE = 128, + SERIAL_RX_TIMEOUT = 2000, + SERIAL_TX_TIMEOUT = 2000, + }; + + raw_serial(); + virtual ~raw_serial(); + virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0); + virtual bool open(); + virtual void close(); + virtual void flush( _u32 flags); + + virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); + + virtual int senddata(const unsigned char * data, size_t size); + virtual int recvdata(unsigned char * data, size_t size); + + virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); + virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); + + virtual size_t rxqueue_count(); + + virtual void setDTR(); + virtual void clearDTR(); + +protected: + bool open(const char * portname, _u32 baudrate, _u32 flags); + void _init(); + + char _portName[20]; + uint32_t _baudrate; + uint32_t _flags; + + OVERLAPPED _ro, _wo; + OVERLAPPED _wait_o; + volatile HANDLE _serial_handle; + DCB _dcb; + COMMTIMEOUTS _co; +}; + +}}} diff --git a/sdk/src/arch/win32/net_socket.cpp b/sdk/src/arch/win32/net_socket.cpp new file mode 100644 index 0000000..611c9d1 --- /dev/null +++ b/sdk/src/arch/win32/net_socket.cpp @@ -0,0 +1,945 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2009 - 2013 RoboPeak Project + * + * Win32 Implementation + */ + +#define _WINSOCKAPI_ + +#include "sdkcommon.h" +#include "..\..\hal\socket.h" +#include +#include +#include + +#include +#include +#pragma comment (lib, "Ws2_32.lib") + +namespace rp{ namespace net { + +static volatile bool _isWSAStartupCalled = false; + +static inline bool _checkWSAStartup() { + int iResult; + WSADATA wsaData; + if (!_isWSAStartupCalled) { + iResult = WSAStartup(MAKEWORD(2,2), &wsaData); + if (iResult != 0) { + return false; + } + _isWSAStartupCalled = true; + } + return true; +} + +static const char* _inet_ntop(int af, const void* src, char* dst, int cnt){ + + struct sockaddr_storage srcaddr; + + + memset(dst, 0, cnt); + + memset(&srcaddr, 0, sizeof(struct sockaddr_storage)); + + + srcaddr.ss_family = af; + + switch (af) { + case AF_INET: + { + struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&srcaddr); + memcpy(&(ipv4->sin_addr), src, sizeof(ipv4->sin_addr)); + } + break; + case AF_INET6: + { + struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&srcaddr); + memcpy(&(ipv6->sin6_addr), src, sizeof(ipv6->sin6_addr)); + } + break; + } + + if (WSAAddressToStringA((struct sockaddr*) &srcaddr, sizeof(struct sockaddr_storage), 0, dst, (LPDWORD) &cnt) != 0) { + DWORD rv = WSAGetLastError(); + return NULL; + } + return dst; +} + +static int _inet_pton(int Family, const char * pszAddrString, void* pAddrBuf) +{ + struct sockaddr_storage tmpholder; + int actualSize = sizeof(sockaddr_storage); + + int result = WSAStringToAddressA((char *)pszAddrString, Family, NULL, (sockaddr*)&tmpholder, &actualSize); + if (result) return -1; + + switch (Family) { + case AF_INET: + { + struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&tmpholder); + memcpy(pAddrBuf, &(ipv4->sin_addr), sizeof(ipv4->sin_addr)); + } + break; + case AF_INET6: + { + struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&tmpholder); + memcpy(pAddrBuf, &(ipv6->sin6_addr), sizeof(ipv6->sin6_addr)); + } + break; + } + return 1; +} + +static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) +{ + switch (type) { + case SocketAddress::ADDRESS_TYPE_INET: + return AF_INET; + case SocketAddress::ADDRESS_TYPE_INET6: + return AF_INET6; + case SocketAddress::ADDRESS_TYPE_UNSPEC: + return AF_UNSPEC; + + default: + assert(!"should not reach here"); + return AF_UNSPEC; + } +} + + +SocketAddress::SocketAddress() +{ + _checkWSAStartup(); + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); +} + + + +SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type) +{ + _checkWSAStartup(); + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_platform_data)->ss_family = AF_INET; + + setAddressFromString(addrString, type); + setPort(port); +} + +SocketAddress::SocketAddress(void * platform_data) + : _platform_data(platform_data) +{ _checkWSAStartup(); } + +SocketAddress & SocketAddress::operator = (const SocketAddress &src) +{ + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); + return *this; +} + + +SocketAddress::~SocketAddress() +{ + delete reinterpret_cast(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_platform_data)->ss_family) { + case AF_INET: + return ADDRESS_TYPE_INET; + case AF_INET6: + return ADDRESS_TYPE_INET6; + default: + assert(!"should not reach here"); + return ADDRESS_TYPE_INET; + } +} + +int SocketAddress::getPort() const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + return (int)ntohs(reinterpret_cast(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_platform_data)->sin6_port = htons((short)port); + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + +u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type) +{ + int ans = 0; + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->ss_family = AF_INET; + ans = _inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = _inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_platform_data)->sin6_addr); + break; + + default: + return RESULT_INVALID_DATA; + + } + setPort(prevPort); + + return ans<=0?RESULT_INVALID_DATA:RESULT_OK; +} + + +u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const +{ + int net_family = reinterpret_cast(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = _inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, (int)buffersize); + break; + + case AF_INET6: + ans = _inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin6_addr, + buffer, (int)buffersize); + + break; + } + return (ans==NULL)?RESULT_OPERATION_FAIL:RESULT_OK; +} + + + +size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS, SocketAddress::address_type_t type) +{ + struct addrinfo hints; + struct addrinfo *result; + int ans; + _checkWSAStartup(); + memset(&hints, 0, sizeof(struct addrinfo)); + hints.ai_family = _halAddrTypeToOSType(type); + hints.ai_flags = AI_PASSIVE; + + if (!performDNS) { + hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST; + + } + + ans = getaddrinfo(hostname, sevicename, &hints, &result); + + addresspool.clear(); + + if (ans != 0) { + // hostname loopup failed + return 0; + } + + + for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) { + if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) { + sockaddr_storage * storagebuffer = new sockaddr_storage; + assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen); + memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen); + addresspool.push_back(SocketAddress(storagebuffer)); + } + } + + + freeaddrinfo(result); + + return addresspool.size(); +} + + +u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + if (bufferSize < sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)) return RESULT_INSUFFICIENT_MEMORY; + + memcpy(buffer, &reinterpret_cast(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)); + + + break; + case ADDRESS_TYPE_INET6: + if (bufferSize < sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)) return RESULT_INSUFFICIENT_MEMORY; + memcpy(buffer, reinterpret_cast(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)); + + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + + +void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type) +{ + + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_loopback; + + } + break; + default: + return; + } + + setPort(prevPort); +} + +void SocketAddress::setBroadcastAddressIPv4() +{ + + int prevPort = getPort(); + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST); + setPort(prevPort); + +} + +void SocketAddress::setAnyAddress(SocketAddress::address_type_t type) +{ + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_ANY); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_any; + + } + break; + default: + return; + } + + setPort(prevPort); + + +} + + +}} + + + +///-------------------------------- + + +namespace rp { namespace arch { namespace net{ + +using namespace rp::net; + +class _single_thread StreamSocketImpl : public StreamSocket +{ +public: + + StreamSocketImpl(SOCKET fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, (int)sizeof(bool_true) ); + + enableNoDelay(true); + this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~StreamSocketImpl() + { + closesocket(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast( const_cast(localaddr.getPlatformData())); //donnot do this at home... + assert(addr); + + int actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, &actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + virtual u_result connect(const SocketAddress & pairAddress) + { + u_long mode_block = 0; + u_long mode_notBlock = 1; + + //set to non block mode + if (SOCKET_ERROR == ioctlsocket(_socket_fd, (long)FIONBIO, &mode_notBlock)) + { + return RESULT_OPERATION_FAIL; + } + + struct timeval tm; + tm.tv_sec = 2; + tm.tv_usec = 0; + int ret = -1; + + const struct sockaddr * addr = reinterpret_cast(pairAddress.getPlatformData()); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + if (!ans) return RESULT_OK; + + fd_set set; + FD_ZERO(&set); + FD_SET(_socket_fd, &set); + + if (select(-1, NULL, &set, NULL, &tm) <= 0) + { + ret = -1; // error(select error or timeout) + return RESULT_OPERATION_TIMEOUT; + } + + int error = -1; + int optLen = sizeof(int); + getsockopt(_socket_fd, SOL_SOCKET, SO_ERROR, (char*)&error, &optLen); + + if (0 != error) + { + ret = -1; // error + } + else + { + ret = 1; // correct + } + + //set back to block mode + if (SOCKET_ERROR == ioctlsocket(_socket_fd, (long)FIONBIO, &mode_block)) + { + return RESULT_OPERATION_FAIL; + } + if(1 == ret) + { + return RESULT_OK; + } + else + { + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result listen(int backlog) + { + int ans = ::listen( _socket_fd, backlog); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual StreamSocket * accept(SocketAddress * pairAddress) + { + int addrsize; + addrsize = sizeof(sockaddr_storage); + SOCKET pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast(const_cast(pairAddress->getPlatformData())):NULL + , &addrsize); + + if (pair_socket>=0) { + return new StreamSocketImpl(pair_socket); + } else { + return NULL; + } + } + + virtual u_result waitforIncomingConnection(_u32 timeout) + { + return waitforData(timeout); + } + + virtual u_result send(const void * buffer, size_t len) + { + int ans = ::send( _socket_fd, (const char *)buffer, (int)len, 0); + if (ans != SOCKET_ERROR ) { + assert(ans == (int)len); + + return RESULT_OK; + } else { + switch(WSAGetLastError()) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + } + + } + + virtual u_result recv(void *buf, size_t len, size_t & recv_len) + { + int ans = ::recv( _socket_fd, (char *)buf, (int)len, 0); + //::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK, (const char *)1, sizeof(int)); + //::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int)) + if (ans == SOCKET_ERROR) { + recv_len = 0; + switch(WSAGetLastError()) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } else { + recv_len = ans; + return RESULT_OK; + } + } + + virtual u_result getPeerAddress(SocketAddress & peerAddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast(peerAddr.getPlatformData())); //donnot do this at home... + assert(addr); + int actualsize = (int)sizeof(sockaddr_storage); + int ans = ::getpeername(_socket_fd, addr, &actualsize); + + assert(actualsize <= (int)sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + + } + + virtual u_result shutdown(socket_direction_mask mask) + { + int shutdw_opt ; + + switch (mask) { + case SOCKET_DIR_RD: + shutdw_opt = SD_RECEIVE; + break; + case SOCKET_DIR_WR: + shutdw_opt = SD_SEND; + break; + case SOCKET_DIR_BOTH: + default: + shutdw_opt = SD_BOTH; + } + + int ans = ::shutdown(_socket_fd, shutdw_opt); + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableKeepAlive(bool enable) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableNoDelay(bool enable ) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY, (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(NULL, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select((int)_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + +protected: + + SOCKET _socket_fd; + + +}; + + +class _single_thread DGramSocketImpl : public DGramSocket +{ +public: + + DGramSocketImpl(SOCKET fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, (int)sizeof(bool_true) ); + setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~DGramSocketImpl() + { + closesocket(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast((localaddr.getPlatformData()))); //donnot do this at home... + assert(addr); + + int actualsize = (int)sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, &actualsize); + + assert(actualsize <= (int)sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (const char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(NULL, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(NULL, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result setPairAddress(const SocketAddress * pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr * addr = pairAddress ? reinterpret_cast(pairAddress->getPlatformData()) : reinterpret_cast(&unspecAddr); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + return ans? RESULT_OPERATION_FAIL: RESULT_OK; + + } + + virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len) + { + + const struct sockaddr * addr = target?reinterpret_cast(target->getPlatformData()): NULL; + int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0); + int ans = ::sendto( _socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size); + if (ans != SOCKET_ERROR) { + assert(ans == (int)len); + return RESULT_OK; + } else { + switch(WSAGetLastError()) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + case WSAEMSGSIZE: + return RESULT_INVALID_DATA; + default: + return RESULT_OPERATION_FAIL; + } + } + + } + + virtual u_result clearRxCache() + { + timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + int res = -1; + char recv_data[2]; + memset(recv_data, 0, sizeof(recv_data)); + while (true) { + res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv); + if (res == 0) break; + recv(_socket_fd, recv_data, 1, 0); + } + return RESULT_OK; + } + + + virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + int source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + int ans = ::recvfrom( _socket_fd, (char *)buf, (int)len, 0, addr, addr?&source_addr_size:NULL); + if (ans == SOCKET_ERROR) { + recv_len = 0; + int errCode = WSAGetLastError(); + switch(errCode) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } else { + recv_len = ans; + return RESULT_OK; + } + + } + + + +protected: + SOCKET _socket_fd; + +}; + + +}}} + + +namespace rp { namespace net{ + + + +static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family) +{ + switch (family) { + case SocketBase::SOCKET_FAMILY_INET: + return AF_INET; + case SocketBase::SOCKET_FAMILY_INET6: + return AF_INET6; + case SocketBase::SOCKET_FAMILY_RAW: + return AF_UNSPEC; //win32 doesn't support RAW Packet + default: + assert(!"should not reach here"); + return AF_INET; // force treating as IPv4 in release mode + } + +} + +StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family) +{ + _checkWSAStartup(); + if (family == SOCKET_FAMILY_RAW) return NULL; + + + int socket_family = _socketHalFamilyToOSFamily(family); + SOCKET socket_fd = ::socket(socket_family, SOCK_STREAM, 0); + if (socket_fd == -1) return NULL; + StreamSocket * newborn = static_cast(new rp::arch::net::StreamSocketImpl(socket_fd)); + return newborn; + +} + + +DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family) +{ + _checkWSAStartup(); + int socket_family = _socketHalFamilyToOSFamily(family); + + + SOCKET socket_fd = ::socket(socket_family, (family == SOCKET_FAMILY_RAW) ? SOCK_RAW : SOCK_DGRAM, 0); + if (socket_fd == -1) return NULL; + DGramSocket * newborn = static_cast(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/sdk/src/arch/win32/timer.cpp b/sdk/src/arch/win32/timer.cpp new file mode 100644 index 0000000..d153cc4 --- /dev/null +++ b/sdk/src/arch/win32/timer.cpp @@ -0,0 +1,72 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include +#pragma comment(lib, "Winmm.lib") + +namespace rp{ namespace arch{ + +static LARGE_INTEGER _current_freq; + +void HPtimer_reset() +{ + BOOL ans=QueryPerformanceFrequency(&_current_freq); + _current_freq.QuadPart/=1000ULL; + assert(ans); +} + +_u64 getHDTimer_us() +{ + LARGE_INTEGER current; + QueryPerformanceCounter(¤t); + + return (_u64)(current.QuadPart / (_current_freq.QuadPart/1000ULL)); + +} + +_u64 getHDTimer() +{ + LARGE_INTEGER current; + QueryPerformanceCounter(¤t); + + return (_u64)(current.QuadPart/_current_freq.QuadPart); +} + +BEGIN_STATIC_CODE(timer_cailb) +{ + HPtimer_reset(); +}END_STATIC_CODE(timer_cailb) + +}} diff --git a/sdk/src/arch/win32/timer.h b/sdk/src/arch/win32/timer.h new file mode 100644 index 0000000..27b7a76 --- /dev/null +++ b/sdk/src/arch/win32/timer.h @@ -0,0 +1,49 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/types.h" + +#define delay(x) ::Sleep(x) + +namespace rp{ namespace arch{ + void HPtimer_reset(); + _u64 getHDTimer(); + _u64 getHDTimer_us(); + +}} + +#define getms() rp::arch::getHDTimer() +#define getus() rp::arch::getHDTimer_us() diff --git a/sdk/src/arch/win32/winthread.hpp b/sdk/src/arch/win32/winthread.hpp new file mode 100644 index 0000000..590794d --- /dev/null +++ b/sdk/src/arch/win32/winthread.hpp @@ -0,0 +1,144 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include + +namespace rp{ namespace hal{ + +Thread Thread::create(thread_proc_t proc, void * data) +{ + Thread newborn(proc, data); + + newborn._handle = (_word_size_t)( + _beginthreadex(NULL, 0, (unsigned int (_stdcall * )( void * ))proc, + data, 0, NULL)); + return newborn; +} + +u_result Thread::terminate() +{ + if (!this->_handle) return RESULT_OK; + if (TerminateThread( reinterpret_cast(this->_handle), -1)) + { + CloseHandle(reinterpret_cast(this->_handle)); + this->_handle = NULL; + return RESULT_OK; + }else + { + return RESULT_OPERATION_FAIL; + } +} + +u_result Thread::SetSelfPriority( priority_val_t p) +{ + HANDLE selfHandle = GetCurrentThread(); + + int win_priority = THREAD_PRIORITY_NORMAL; + switch(p) + { + case PRIORITY_REALTIME: + win_priority = THREAD_PRIORITY_TIME_CRITICAL; + break; + case PRIORITY_HIGH: + win_priority = THREAD_PRIORITY_HIGHEST; + break; + case PRIORITY_NORMAL: + win_priority = THREAD_PRIORITY_NORMAL; + break; + case PRIORITY_LOW: + win_priority = THREAD_PRIORITY_LOWEST; + break; + case PRIORITY_IDLE: + win_priority = THREAD_PRIORITY_IDLE; + break; + } + + if (SetThreadPriority(selfHandle, win_priority)) + { + return RESULT_OK; + } + return RESULT_OPERATION_FAIL; +} + +Thread::priority_val_t Thread::getPriority() +{ + if (!this->_handle) return PRIORITY_NORMAL; + int win_priority = ::GetThreadPriority(reinterpret_cast(this->_handle)); + + if (win_priority == THREAD_PRIORITY_ERROR_RETURN) + { + return PRIORITY_NORMAL; + } + + if (win_priority >= THREAD_PRIORITY_TIME_CRITICAL ) + { + return PRIORITY_REALTIME; + } + else if (win_priority=THREAD_PRIORITY_ABOVE_NORMAL) + { + return PRIORITY_HIGH; + } + else if (win_priorityTHREAD_PRIORITY_BELOW_NORMAL) + { + return PRIORITY_NORMAL; + }else if (win_priority<=THREAD_PRIORITY_BELOW_NORMAL && win_priority>THREAD_PRIORITY_IDLE) + { + return PRIORITY_LOW; + }else if (win_priority<=THREAD_PRIORITY_IDLE) + { + return PRIORITY_IDLE; + } + return PRIORITY_NORMAL; +} + +u_result Thread::join(unsigned long timeout) +{ + if (!this->_handle) return RESULT_OK; + switch ( WaitForSingleObject(reinterpret_cast(this->_handle), timeout)) + { + case WAIT_OBJECT_0: + CloseHandle(reinterpret_cast(this->_handle)); + this->_handle = NULL; + return RESULT_OK; + case WAIT_ABANDONED: + return RESULT_OPERATION_FAIL; + case WAIT_TIMEOUT: + return RESULT_OPERATION_TIMEOUT; + } + + return RESULT_OK; +} + +}} diff --git a/sdk/src/dataunpacker/dataunnpacker_commondef.h b/sdk/src/dataunpacker/dataunnpacker_commondef.h new file mode 100644 index 0000000..70b9c99 --- /dev/null +++ b/sdk/src/dataunpacker/dataunnpacker_commondef.h @@ -0,0 +1,60 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * External Reference and dependencies + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "hal/waiter.h" +#include "hal/byteorder.h" +#include "sl_lidar_driver.h" +#include "sl_crc.h" +#include +#include + +#define CONF_NO_BOOST_CRC_SUPPORT + +#include "dataupacker_namespace.h" + + diff --git a/sdk/src/dataunpacker/dataunnpacker_internal.h b/sdk/src/dataunpacker/dataunnpacker_internal.h new file mode 100644 index 0000000..6b1b6dd --- /dev/null +++ b/sdk/src/dataunpacker/dataunnpacker_internal.h @@ -0,0 +1,74 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Internal Definition + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + + +class LIDARSampleDataUnpackerInner: public LIDARSampleDataUnpacker +{ +public: + LIDARSampleDataUnpackerInner(LIDARSampleDataListener& l): LIDARSampleDataUnpacker(l){} + + virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0; + virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) = 0; + virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) = 0; + virtual void publishNewScanReset() = 0; + + + virtual _u64 getCurrentTimestamp_uS() = 0; + +}; + +class IDataUnpackerHandler +{ +public: + IDataUnpackerHandler() {} + virtual ~IDataUnpackerHandler() {} + + + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) = 0; + + virtual _u8 getSampleAnswerType() const = 0; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size) = 0; + virtual void reset() = 0; + +}; + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/dataunpacker.cpp b/sdk/src/dataunpacker/dataunpacker.cpp new file mode 100644 index 0000000..74680c0 --- /dev/null +++ b/sdk/src/dataunpacker/dataunpacker.cpp @@ -0,0 +1,259 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "dataunnpacker_commondef.h" +#include "dataunpacker.h" +#include "dataunnpacker_internal.h" + + +#include + + +#define REGISTER_HANDLER(_c_) { \ + auto newBorn = new unpacker::_c_(); \ + if (!newBorn) return false; \ + handlerList.push_back(newBorn); \ + } + +// How to include new handlers? +// 1. add extra include line below if a new handle is to be included +// 2. update the code in function _registerDataUnpackerHandlers +#include "unpacker/handler_capsules.h" +#include "unpacker/handler_hqnode.h" +#include "unpacker/handler_normalnode.h" + + +#define DEF_REGISTER_HANDLER_LIST + + +BEGIN_DATAUNPACKER_NS() + + +static bool _registerDataUnpackerHandlers(std::vector & handlerList) +{ + REGISTER_HANDLER(UnpackerHandler_NormalNode); + REGISTER_HANDLER(UnpackerHandler_HQNode); + REGISTER_HANDLER(UnpackerHandler_CapsuleNode); + REGISTER_HANDLER(UnpackerHandler_UltraCapsuleNode); + REGISTER_HANDLER(UnpackerHandler_DenseCapsuleNode); + REGISTER_HANDLER(UnpackerHandler_UltraDenseCapsuleNode); + return true; +} + + +class LIDARSampleDataUnpackerImpl : public LIDARSampleDataUnpackerInner +{ +public: + + void registerHandler(_u8 ansType, IDataUnpackerHandler* handler) + { + _handlerMap[ansType] = handler; + } + + + void unregisterAllHandlers() + { + for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr) + { + delete itr->second; + } + _handlerMap.clear(); + } + + LIDARSampleDataUnpackerImpl(LIDARSampleDataListener& l) + : LIDARSampleDataUnpackerInner(l) + , _enabled(false) + , _lastActiveAnsType(0) + , _lastActiveHandler(nullptr) + { + + } + + virtual ~LIDARSampleDataUnpackerImpl() + { + unregisterAllHandlers(); + } + + + virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) + { + + // notify the handlers ... + for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr) + { + itr->second->onUnpackerContextSet(type, data, size); + } + } + + virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) { + if (!_enabled) return false; + + + if (_lastActiveAnsType != ansType) { + onDeselectHandler(); + + auto itr = _handlerMap.find(ansType); + if (itr != _handlerMap.end()) { + onSelectHandler(ansType, itr->second); + } + else { + onSelectHandler(ansType, nullptr); + } + + } + + if (_lastActiveHandler) { + _lastActiveHandler->onData(this, reinterpret_cast(buffer), size); + return true; + } + else { + return false; + } + } + + virtual void reset() + { + clearCache(); + _lastActiveHandler = nullptr; + _lastActiveAnsType = 0; + + } + + virtual void enable() + { + _enabled = true; + reset(); + } + + virtual void disable() + { + _enabled = false; + reset(); + + } + + virtual void clearCache() + { + if (_lastActiveHandler) { + _lastActiveHandler->reset(); + } + } + + virtual _u64 getCurrentTimestamp_uS() { + return getus(); + } + + virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) + { + _listener.onHQNodeDecoded(timestamp_uS, node); + } + + + virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) + { + _listener.onDecodingError(errorType, ansType, payload, size); + + } + + virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) + { + _listener.onCustomSampleDataDecoded(ansType, customCode, payload, size); + } + + + virtual void publishNewScanReset() + { + _listener.onHQNodeScanResetReq(); + } +protected: + + void onSelectHandler(_u8 ansType, IDataUnpackerHandler* handler) + { + _lastActiveHandler = handler; + _lastActiveAnsType = ansType; + } + + void onDeselectHandler() + { + reset(); + } + + +protected: + bool _enabled; + std::map<_u8, IDataUnpackerHandler*> _handlerMap; + + _u8 _lastActiveAnsType; + IDataUnpackerHandler* _lastActiveHandler; +}; + +LIDARSampleDataUnpacker* LIDARSampleDataUnpacker::CreateInstance(LIDARSampleDataListener& listener) +{ + LIDARSampleDataUnpackerImpl* impl = new LIDARSampleDataUnpackerImpl(listener); + + std::vector list; + if (!_registerDataUnpackerHandlers(list)) { + delete impl; + for (auto itr = list.begin(); itr != list.end(); ++itr) { + delete* itr; + } + impl = nullptr; + } + + for (auto itr = list.begin(); itr != list.end(); ++itr) { + impl->registerHandler((*itr)->getSampleAnswerType(), (*itr)); + } + return impl; +} + +void LIDARSampleDataUnpacker::ReleaseInstance(LIDARSampleDataUnpacker* unpacker) { + delete unpacker; +} + +LIDARSampleDataUnpacker::~LIDARSampleDataUnpacker() { + +} + +LIDARSampleDataUnpacker::LIDARSampleDataUnpacker(LIDARSampleDataListener& l) + : _listener(l) +{ + +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/dataunpacker.h b/sdk/src/dataunpacker/dataunpacker.h new file mode 100644 index 0000000..2cdf6ca --- /dev/null +++ b/sdk/src/dataunpacker/dataunpacker.h @@ -0,0 +1,93 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + + +#pragma once + +#include "dataupacker_namespace.h" + +BEGIN_DATAUNPACKER_NS() + + +class LIDARSampleDataListener +{ + + +public: + virtual void onHQNodeScanResetReq() = 0; + virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0; + virtual void onCustomSampleDataDecoded(_u8 ansType, _u32 customCode, const void* data, size_t size) {} + + virtual void onDecodingError(int errMsg, _u8 ansType, const void* payload, size_t size) {} +}; + +class LIDARSampleDataUnpacker +{ +public: + enum { + ERR_EVENT_ON_EXP_ENCODER_RESET = 0x8001, + ERR_EVENT_ON_EXP_CHECKSUM_ERR = 0x8002, + }; + + enum UnpackerContextType { + UNPACKER_CONTEXT_TYPE_LIDAR_UNKNOWN = 0, + UNPACKER_CONTEXT_TYPE_LIDAR_TIMING = 1, + UNPACKER_CONTEXT_TYPE_TRIANGULATION_OPTICAL_FACTOR = 2, + + }; + + virtual ~LIDARSampleDataUnpacker(); + static LIDARSampleDataUnpacker* CreateInstance(LIDARSampleDataListener& listener); + static void ReleaseInstance(LIDARSampleDataUnpacker*); + + virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) = 0; + + virtual void enable() = 0; + virtual void disable() = 0; + + virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) = 0; + virtual void reset() = 0; + virtual void clearCache() = 0; + +protected: + LIDARSampleDataUnpacker(LIDARSampleDataListener&); + LIDARSampleDataListener& _listener; + +}; + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/dataupacker_namespace.h b/sdk/src/dataunpacker/dataupacker_namespace.h new file mode 100644 index 0000000..9e27192 --- /dev/null +++ b/sdk/src/dataunpacker/dataupacker_namespace.h @@ -0,0 +1,5 @@ +#pragma once + + +#define BEGIN_DATAUNPACKER_NS() namespace sl{ namespace internal{ +#define END_DATAUNPACKER_NS() }} \ No newline at end of file diff --git a/sdk/src/dataunpacker/unpacker/handler_capsules.cpp b/sdk/src/dataunpacker/unpacker/handler_capsules.cpp new file mode 100644 index 0000000..ab3110d --- /dev/null +++ b/sdk/src/dataunpacker/unpacker/handler_capsules.cpp @@ -0,0 +1,1054 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Capsule Style Sample Node Handlers + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "../dataunnpacker_commondef.h" +#include "../dataunpacker.h" +#include "../dataunnpacker_internal.h" + + + +#include "handler_capsules.h" + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + + +// UnpackerHandler_CapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInExpressMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = (31 - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + + +UnpackerHandler_CapsuleNode::UnpackerHandler_CapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_CapsuleNode::~UnpackerHandler_CapsuleNode() +{ + +} + +void UnpackerHandler_CapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_CapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; +} + +void UnpackerHandler_CapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) { + node->cabins[cpos].distance_angle_1 = le16_to_cpu(node->cabins[cpos].distance_angle_1); + node->cabins[cpos].distance_angle_2 = le16_to_cpu(node->cabins[cpos].distance_angle_2); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED, node, sizeof(*node)); + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + engine->publishNewScanReset(); + + + } + _onScanNodeCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + +void UnpackerHandler_CapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + _cached_last_data_timestamp_us = 0; +} + +void UnpackerHandler_CapsuleNode::_onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine) +{ + _u64 currentTS = engine->getCurrentTimestamp_uS(); + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3); + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_capsuledata.cabins); ++pos) + { + int dist_q2[2]; + int angle_q6[2]; + int syncBit[2]; + + dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); + dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); + + int angle_offset1_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3) << 4)); + int angle_offset2_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3) << 4)); + + angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10); + syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + + angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10); + syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + for (int cpos = 0; cpos < 2; ++cpos) { + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + rplidar_response_measurement_node_hq_t hqNode; + + + hqNode.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + hqNode.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + + hqNode.angle_z_q14 = (angle_q6[cpos] << 8) / 90; + hqNode.dist_mm_q2 = dist_q2[cpos]; + + engine->publishHQNode(_cached_last_data_timestamp_us - _getSampleDelayOffsetInExpressMode(_cachedTimingDesc, pos * 2 + cpos), &hqNode); + } + + } + } + + _cached_previous_capsuledata = capsule; + _is_previous_capsuledataRdy = true; + _cached_last_data_timestamp_us = currentTS; + +} + + +// UnpackerHandler_UltraCapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInUltraBoostMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 256000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = ((32 * 3 - 1) - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + + +UnpackerHandler_UltraCapsuleNode::UnpackerHandler_UltraCapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_UltraCapsuleNode::~UnpackerHandler_UltraCapsuleNode() +{ + +} + +void UnpackerHandler_UltraCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_UltraCapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA; +} + +void UnpackerHandler_UltraCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_ultra_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->ultra_cabins); ++cpos) { + node->ultra_cabins[cpos].combined_x3 = le32_to_cpu(node->ultra_cabins[cpos].combined_x3); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA, node, sizeof(*node)); + + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + + engine->publishNewScanReset(); + + } + _onScanNodeUltraCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + +void UnpackerHandler_UltraCapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; +} + +static _u32 _varbitscale_decode(_u32 scaled, _u32& scaleLevel) +{ + static const _u32 VBS_SCALED_BASE[] = { + RPLIDAR_VARBITSCALE_X16_DEST_VAL, + RPLIDAR_VARBITSCALE_X8_DEST_VAL, + RPLIDAR_VARBITSCALE_X4_DEST_VAL, + RPLIDAR_VARBITSCALE_X2_DEST_VAL, + 0, + }; + + static const _u32 VBS_SCALED_LVL[] = { + 4, + 3, + 2, + 1, + 0, + }; + + static const _u32 VBS_TARGET_BASE[] = { + (0x1 << RPLIDAR_VARBITSCALE_X16_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X8_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X4_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X2_SRC_BIT), + 0, + }; + + for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i) + { + int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]); + if (remain >= 0) { + scaleLevel = VBS_SCALED_LVL[i]; + return VBS_TARGET_BASE[i] + (remain << scaleLevel); + } + } + + return 0; +} + +void UnpackerHandler_UltraCapsuleNode::_onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine) +{ + _u64 currentTS = engine->getCurrentTimestamp_uS(); + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3) / 3; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos) + { + int dist_q2[3]; + int angle_q6[3]; + int syncBit[3]; + + + _u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3; + + // unpack ... + int dist_major = (combined_x3 & 0xFFF); + + // signed partical integer, using the magic shift here + // DO NOT TOUCH + + int dist_predict1 = (((int)(combined_x3 << 10)) >> 22); + int dist_predict2 = (((int)combined_x3) >> 22); + + int dist_major2; + + _u32 scalelvl1=0, scalelvl2 = 0; + + // prefetch next ... + if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1) + { + dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF); + } + else { + dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF); + } + + // decode with the var bit scale ... + dist_major = _varbitscale_decode(dist_major, scalelvl1); + dist_major2 = _varbitscale_decode(dist_major2, scalelvl2); + + + int dist_base1 = dist_major; + int dist_base2 = dist_major2; + + if ((!dist_major) && dist_major2) { + dist_base1 = dist_major2; + scalelvl1 = scalelvl2; + } + + + dist_q2[0] = (dist_major << 2); + if (((_u32)dist_predict1 == 0xFFFFFE00) || ((_u32)dist_predict1 == 0x1FF)) { + dist_q2[1] = 0; + } + else { + dist_predict1 = (int)(dist_predict1 << scalelvl1); + dist_q2[1] = (dist_predict1 + dist_base1) << 2; + + } + + if (((_u32)dist_predict2 == 0xFFFFFE00) || ((_u32)dist_predict2 == 0x1FF)) { + dist_q2[2] = 0; + } + else { + dist_predict2 = (int)(dist_predict2 << scalelvl2); + dist_q2[2] = (dist_predict2 + dist_base2) << 2; + } + + for (int cpos = 0; cpos < 3; ++cpos) + { + + syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + + + rplidar_response_measurement_node_hq_t hqNode; + + + int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); + + if (dist_q2[cpos] >= (50 * 4)) + { + const int k1 = 98361; + const int k2 = int(k1 / dist_q2[cpos]); + + offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304; + } + + angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10); + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + + hqNode.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + hqNode.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + + hqNode.angle_z_q14 = (angle_q6[cpos] << 8) / 90; + hqNode.dist_mm_q2 = dist_q2[cpos]; + + engine->publishHQNode(_cached_last_data_timestamp_us - _getSampleDelayOffsetInUltraBoostMode(_cachedTimingDesc, pos * 3 + cpos), &hqNode); + } + + } + } + + _cached_previous_ultracapsuledata = capsule; + _is_previous_capsuledataRdy = true; + _cached_last_data_timestamp_us = currentTS; + +} + + +// UnpackerHandler_DenseCapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInDenseMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 256000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_dense_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = (39 - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + +UnpackerHandler_DenseCapsuleNode::UnpackerHandler_DenseCapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) + +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_dense_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_DenseCapsuleNode::~UnpackerHandler_DenseCapsuleNode() +{ + +} + +void UnpackerHandler_DenseCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_DenseCapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED; +} + + +void UnpackerHandler_DenseCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_dense_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_dense_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_dense_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_dense_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_dense_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) { + node->cabins[cpos].distance_angle_1 = le16_to_cpu(node->cabins[cpos].distance_angle_1); + node->cabins[cpos].distance_angle_2 = le16_to_cpu(node->cabins[cpos].distance_angle_2); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED, node, sizeof(*node)); + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + engine->publishNewScanReset(); + + + } + _onScanNodeDenseCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } +} + +void UnpackerHandler_DenseCapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _cached_last_data_timestamp_us = 0; +} + +void UnpackerHandler_DenseCapsuleNode::_onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t& dense_capsule, LIDARSampleDataUnpackerInner* engine) +{ + static int lastNodeSyncBit = 0; + _u64 currentTs = engine->getCurrentTimestamp_uS(); + + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((dense_capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + int maxDiffAngleThreshold_q8 = (360/* 360 degree */ * 100 /*100Hz*/ * _countof(dense_capsule.cabins) /*40 points per capsule*/ / (1000000 / _cachedTimingDesc.sample_duration_uS)) << 8; + if (diffAngle_q8 > maxDiffAngleThreshold_q8) {//discard + _cached_previous_dense_capsuledata = dense_capsule; + return; + } + + int angleInc_q16 = (diffAngle_q8 << 8) / 40; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_dense_capsuledata.cabins); ++pos) + { + int dist_q2; + int angle_q6; + int syncBit; + const int dist = static_cast(_cached_previous_dense_capsuledata.cabins[pos].distance); + dist_q2 = dist << 2; + angle_q6 = (currentAngle_raw_q16 >> 10); + syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16 << 1)) ? 1 : 0; + syncBit = (syncBit ^ lastNodeSyncBit) & syncBit;//Ensure that syncBit is exactly detected + + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6 < 0) angle_q6 += (360 << 6); + if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); + + rplidar_response_measurement_node_hq_t hqNode; + + + hqNode.flag = (syncBit | ((!syncBit) << 1)); + hqNode.quality = dist_q2 ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + hqNode.angle_z_q14 = (angle_q6 << 8) / 90; + hqNode.dist_mm_q2 = dist_q2; + engine->publishHQNode(currentTs - _getSampleDelayOffsetInDenseMode(_cachedTimingDesc, pos), &hqNode); + + lastNodeSyncBit = syncBit; + + } + } + + _cached_previous_dense_capsuledata = dense_capsule; + _is_previous_capsuledataRdy = true; + +} + +// UnpackerHandler_UltraDenseCapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInUltraDenseMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 1000000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(sl_lidar_response_ultra_dense_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = (31 - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + + +UnpackerHandler_UltraDenseCapsuleNode::UnpackerHandler_UltraDenseCapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) + , _last_node_sync_bit(0) + , _last_dist_q2(0) + +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_UltraDenseCapsuleNode::~UnpackerHandler_UltraDenseCapsuleNode() +{ + +} + + +void UnpackerHandler_UltraDenseCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_UltraDenseCapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED; +} + +void UnpackerHandler_UltraDenseCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_ultra_dense_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_ultra_dense_capsule_measurement_nodes_t, time_stamp); + cpos < sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) { + node->cabins[cpos].qualityl_distance_scale[0] = le16_to_cpu(node->cabins[cpos].qualityl_distance_scale[0]); + node->cabins[cpos].qualityl_distance_scale[1] = le16_to_cpu(node->cabins[cpos].qualityl_distance_scale[1]); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED, node, sizeof(*node)); + + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + engine->publishNewScanReset(); + + } + _onScanNodeUltraDenseCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + +void UnpackerHandler_UltraDenseCapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _cached_last_data_timestamp_us = 0; + _last_node_sync_bit = 0; + _last_dist_q2 = 0; +} + +void UnpackerHandler_UltraDenseCapsuleNode::_onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine) +{ + _u64 currentTimestamp = engine->getCurrentTimestamp_uS(); + + const rplidar_response_ultra_dense_capsule_measurement_nodes_t* ultra_dense_capsule = reinterpret_cast(&capsule); + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((ultra_dense_capsule->start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_ultra_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int maxDiffAngleThreshold_q8 = (360/* 360 degree */ * 100 /*100Hz*/ * _countof(ultra_dense_capsule->cabins) /*64 points per capsule*/ / (1000000 / _cachedTimingDesc.sample_duration_uS)) << 8; + if (diffAngle_q8 > maxDiffAngleThreshold_q8) {//discard + _cached_previous_ultra_dense_capsuledata = *ultra_dense_capsule; + return; + } +#define DISTANCE_THRESHOLD_TO_SCALE_1 2046 // (2^10 - 1)*2 mm +#define DISTANCE_THRESHOLD_TO_SCALE_2 8187 // (2^11 - 1)*3 + 2046 mm +#define DISTANCE_THRESHOLD_TO_SCALE_3 24567 // (2^12 - 1)*4 + 8187 mm + int angleInc_q16 = (diffAngle_q8 << 8) / 64; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_ultra_dense_capsuledata.cabins) * 2; ++pos) + { + int angle_q6; + int syncBit; + size_t cabin_idx = pos >> 1; + _u32 quality_dist_scale; + if (!(pos & 0x1)) { + quality_dist_scale = _cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityl_distance_scale[0] | ((_cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityh_array & 0x0F) << 16); + } + else { + quality_dist_scale = _cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityl_distance_scale[1] | ((_cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityh_array >> 4) << 16); + } + + _u8 scale = quality_dist_scale & 0x3; + _u8 quality = 0; + int dist_q2 = 0; + + switch (scale) { + case 0: + quality = quality_dist_scale >> 12; + dist_q2 = (quality_dist_scale & 0xFFC) * 2; + if (_last_dist_q2) { + if (abs(dist_q2 - _last_dist_q2) <= 8/*2mm *2*/) { + dist_q2 = (dist_q2 + _last_dist_q2) >> 1; + } + } + break; + case 1: + quality = (quality_dist_scale >> 13) << 1; + dist_q2 = (quality_dist_scale & 0x1FFC) * 3 + (DISTANCE_THRESHOLD_TO_SCALE_1 << 2); + break; + case 2: + quality = (quality_dist_scale >> 14) << 2; + dist_q2 = (quality_dist_scale & 0x3FFC) * 4 + (DISTANCE_THRESHOLD_TO_SCALE_2 << 2); + break; + case 3: + quality = (quality_dist_scale >> 15) << 3; + dist_q2 = (quality_dist_scale & 0x7FFC) * 5 + (DISTANCE_THRESHOLD_TO_SCALE_3 << 2); + break; + } + _last_dist_q2 = dist_q2; + angle_q6 = (currentAngle_raw_q16 >> 10); + syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16 << 1)) ? 1 : 0; + syncBit = (syncBit ^ _last_node_sync_bit) & syncBit;//Ensure that syncBit is exactly detected + + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6 < 0) angle_q6 += (360 << 6); + if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); + + + rplidar_response_measurement_node_hq_t hqNode; + + + + hqNode.flag = (syncBit | ((!syncBit) << 1)); + hqNode.quality = quality; + hqNode.angle_z_q14 = (angle_q6 << 8) / 90; + hqNode.dist_mm_q2 = dist_q2; + engine->publishHQNode(currentTimestamp - _getSampleDelayOffsetInUltraDenseMode(_cachedTimingDesc, pos), &hqNode); + + _last_node_sync_bit = syncBit; + + } + } + + _cached_previous_ultra_dense_capsuledata = *ultra_dense_capsule; + _is_previous_capsuledataRdy = true; + +} + + + +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/unpacker/handler_capsules.h b/sdk/src/dataunpacker/unpacker/handler_capsules.h new file mode 100644 index 0000000..008f544 --- /dev/null +++ b/sdk/src/dataunpacker/unpacker/handler_capsules.h @@ -0,0 +1,149 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Capsule Style Sample Node Handlers + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker { + +class UnpackerHandler_CapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_CapsuleNode(); + virtual ~UnpackerHandler_CapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + + void _onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t &, LIDARSampleDataUnpackerInner* engine); + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + _u64 _cached_last_data_timestamp_us; + + SlamtecLidarTimingDesc _cachedTimingDesc; +}; + +class UnpackerHandler_UltraCapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_UltraCapsuleNode(); + virtual ~UnpackerHandler_UltraCapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + void _onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine); + + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + _u64 _cached_last_data_timestamp_us; + + SlamtecLidarTimingDesc _cachedTimingDesc; + +}; + + + +class UnpackerHandler_DenseCapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_DenseCapsuleNode(); + virtual ~UnpackerHandler_DenseCapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + void _onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine); + + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; + _u64 _cached_last_data_timestamp_us; + + SlamtecLidarTimingDesc _cachedTimingDesc; + +}; + + +class UnpackerHandler_UltraDenseCapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_UltraDenseCapsuleNode(); + virtual ~UnpackerHandler_UltraDenseCapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + void _onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine); + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_ultra_dense_capsule_measurement_nodes_t _cached_previous_ultra_dense_capsuledata; + _u64 _cached_last_data_timestamp_us; + + + + int _last_node_sync_bit; + int _last_dist_q2; + + SlamtecLidarTimingDesc _cachedTimingDesc; +}; + + +} + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp b/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp new file mode 100644 index 0000000..8ed450e --- /dev/null +++ b/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp @@ -0,0 +1,192 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * HQNode Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "../dataunnpacker_commondef.h" +#include "../dataunpacker.h" +#include "../dataunnpacker_internal.h" + +#ifdef CONF_NO_BOOST_CRC_SUPPORT +#include "sl_crc.h" +#endif + +#include "handler_hqnode.h" + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + + +static _u64 _getSampleDelayOffsetInHQMode(const SlamtecLidarTimingDesc& timing) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:1000000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_hq_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS; +} + +UnpackerHandler_HQNode::UnpackerHandler_HQNode() + : _cached_scan_node_buf_pos(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_HQNode::~UnpackerHandler_HQNode() +{ + +} + +_u8 UnpackerHandler_HQNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_HQ; +} + +void UnpackerHandler_HQNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + + for (size_t pos = 0; pos < cnt; ++pos) + { + _u8 current_data = data[pos]; + + switch (_cached_scan_node_buf_pos) + { + case 0: // expect the sync byte + { + if (current_data == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC) { + // pass + } + else { + continue; + } + } + break; + + case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: // get bytes to calculate crc ready + { + + } + break; + + case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + rplidar_response_hq_capsule_measurement_nodes_t* nodesData = reinterpret_cast(&_cached_scan_node_buf[0]); + +#ifdef CONF_NO_BOOST_CRC_SUPPORT + _u32 crcCalc = crc32::getResult(&_cached_scan_node_buf[0], sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4); + + +#else + // calculate crc with boost crc method + boost::crc_optimal<32, 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true> mycrc; + std::vector<_u8> crcInputData; + crcInputData.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4); + memcpy(&crcInputData[0], nodesData, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4); + //supplement crcInputData to mutiples of 4 + int leftBytes = 4 - (crcInputData.size() & 3); + for (int i = 0; i < leftBytes; i++) + crcInputData.push_back(0); + mycrc.process_bytes(&crcInputData[0], crcInputData.size()); + _u32 crcCalc = mycrc.checksum(); + +#endif + + _u32 recvCRC = nodesData->crc32; +#ifdef _CPU_ENDIAN_BIG + recvCRC = le32_to_cpu(recvCRC); + nodesData->time_stamp = le64_to_cpu(nodesData->time_stamp); +#endif + if (recvCRC == crcCalc) + { + for (size_t pos = 0; pos < _countof(nodesData->node_hq); ++pos) + { + rplidar_response_measurement_node_hq_t hqNode = nodesData->node_hq[pos]; +#ifdef _CPU_ENDIAN_BIG + hqNode.angle_z_q14 = le16_to_cpu(hqNode.angle_z_q14); + hqNode.dist_mm_q2 = le32_to_cpu(hqNode.dist_mm_q2); +#endif + engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInHQMode(_cachedTimingDesc), &hqNode); + } + } + else //crc check not passed + { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_HQ, nodesData, sizeof(*nodesData)); + } + continue; + } + break; + + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + + +void UnpackerHandler_HQNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + +void UnpackerHandler_HQNode::reset() +{ + _cached_scan_node_buf_pos = 0; +} +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/unpacker/handler_hqnode.h b/sdk/src/dataunpacker/unpacker/handler_hqnode.h new file mode 100644 index 0000000..178a8c6 --- /dev/null +++ b/sdk/src/dataunpacker/unpacker/handler_hqnode.h @@ -0,0 +1,63 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * HQNode Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker { + + class UnpackerHandler_HQNode : public IDataUnpackerHandler { + public: + UnpackerHandler_HQNode(); + virtual ~UnpackerHandler_HQNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); + + protected: + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + SlamtecLidarTimingDesc _cachedTimingDesc; + }; + +} + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp b/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp new file mode 100644 index 0000000..b799b77 --- /dev/null +++ b/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp @@ -0,0 +1,159 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Normal Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "../dataunnpacker_commondef.h" +#include "../dataunpacker.h" +#include "../dataunnpacker_internal.h" + + +#include "handler_normalnode.h" + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + + +static _u64 _getSampleDelayOffsetInLegacyMode(const SlamtecLidarTimingDesc& timing) +{ + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS; +} + +UnpackerHandler_NormalNode::UnpackerHandler_NormalNode() + : _cached_scan_node_buf_pos(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_measurement_node_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +;} + +UnpackerHandler_NormalNode::~UnpackerHandler_NormalNode() +{ + +} + +_u8 UnpackerHandler_NormalNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT; +} + +void UnpackerHandler_NormalNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit and its reverse in this byte + { + _u8 tmp = (current_data >> 1); + if ((tmp ^ current_data) & 0x1) { + // pass + } + else { + continue; + } + + } + break; + case 1: // expect the highest bit to be 1 + { + if (current_data & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + continue; + } + } + break; + case sizeof(rplidar_response_measurement_node_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_measurement_node_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_measurement_node_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); +#ifdef _CPU_ENDIAN_BIG + node->angle_q6_checkbit = le16_to_cpu(node->angle_q6_checkbit); + node->distance_q2 = le16_to_cpu(node->distance_q2); +#endif + //cast node to rplidar_response_measurement_node_hq_t + rplidar_response_measurement_node_hq_t hqNode; + hqNode.angle_z_q14 = (((node->angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle + hqNode.dist_mm_q2 = node->distance_q2; + hqNode.flag = (node->sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field + hqNode.quality = (node->sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 + + + engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInLegacyMode(_cachedTimingDesc), &hqNode); + continue; + + } + break; + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } +} + + +void UnpackerHandler_NormalNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + +void UnpackerHandler_NormalNode::reset() +{ + _cached_scan_node_buf_pos = 0; +} +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/dataunpacker/unpacker/handler_normalnode.h b/sdk/src/dataunpacker/unpacker/handler_normalnode.h new file mode 100644 index 0000000..9068b1b --- /dev/null +++ b/sdk/src/dataunpacker/unpacker/handler_normalnode.h @@ -0,0 +1,63 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Normal Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + +class UnpackerHandler_NormalNode : public IDataUnpackerHandler { +public: + UnpackerHandler_NormalNode(); + virtual ~UnpackerHandler_NormalNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + + SlamtecLidarTimingDesc _cachedTimingDesc; +}; + +} + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/sdk/src/hal/abs_rxtx.h b/sdk/src/hal/abs_rxtx.h new file mode 100644 index 0000000..54900e3 --- /dev/null +++ b/sdk/src/hal/abs_rxtx.h @@ -0,0 +1,88 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/types.h" + +namespace rp{ namespace hal{ + +class serial_rxtx +{ +public: + enum{ + ANS_OK = 0, + ANS_TIMEOUT = -1, + ANS_DEV_ERR = -2, + }; + + static serial_rxtx * CreateRxTx(); + static void ReleaseRxTx( serial_rxtx * ); + + serial_rxtx():_is_serial_opened(false){} + virtual ~serial_rxtx(){} + + virtual void flush( _u32 flags) = 0; + + virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0) = 0; + virtual bool open() = 0; + virtual void close() = 0; + + virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0; + + virtual int senddata(const unsigned char * data, size_t size) = 0; + virtual int recvdata(unsigned char * data, size_t size) = 0; + + virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL) = 0; + virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL) = 0; + + virtual size_t rxqueue_count() = 0; + + virtual void setDTR() = 0; + virtual void clearDTR() = 0; + virtual void cancelOperation() {} + + virtual bool isOpened() + { + return _is_serial_opened; + } + +protected: + volatile bool _is_serial_opened; +}; + +}} + + + diff --git a/sdk/src/hal/assert.h b/sdk/src/hal/assert.h new file mode 100644 index 0000000..37cfdd5 --- /dev/null +++ b/sdk/src/hal/assert.h @@ -0,0 +1,18 @@ +#ifndef _INFRA_HAL_ASSERT_H +#define _INFRA_HAL_ASSERT_H + +#ifdef WIN32 +#include +#ifndef assert +#define assert(x) _ASSERT(x) +#endif +#elif defined(_AVR_) +#define assert(x) +#elif defined(__GNUC__) +#ifndef assert +#define assert(x) +#endif +#else +#error assert.h cannot identify your platform +#endif +#endif diff --git a/sdk/src/hal/byteops.h b/sdk/src/hal/byteops.h new file mode 100644 index 0000000..c86bde3 --- /dev/null +++ b/sdk/src/hal/byteops.h @@ -0,0 +1,94 @@ +/* + * RoboPeak Project + * Copyright 2009 - 2013 + * + * RPOS - Byte Operations + * + */ + +#pragma once + +// byte swapping operations for compiling time + +#define __static_byteswap_16(x) ((_u16)( \ + (((_u16)(x) & (_u16)0x00FFU) << 8) | \ + (((_u16)(x) & (_u16)0xFF00U) >> 8))) + +#define __static_byteswap_32(x) ((_u32)( \ + (((_u32)(x) & (_u32)0x000000FFUL) << 24) | \ + (((_u32)(x) & (_u32)0x0000FF00UL) << 8) | \ + (((_u32)(x) & (_u32)0x00FF0000UL) >> 8) | \ + (((_u32)(x) & (_u32)0xFF000000UL) >> 24))) + +#define __static_byteswap_64(x) ((_u64)( \ + (((_u64)(x) & (_u64)0x00000000000000ffULL) << 56) | \ + (((_u64)(x) & (_u64)0x000000000000ff00ULL) << 40) | \ + (((_u64)(x) & (_u64)0x0000000000ff0000ULL) << 24) | \ + (((_u64)(x) & (_u64)0x00000000ff000000ULL) << 8) | \ + (((_u64)(x) & (_u64)0x000000ff00000000ULL) >> 8) | \ + (((_u64)(x) & (_u64)0x0000ff0000000000ULL) >> 24) | \ + (((_u64)(x) & (_u64)0x00ff000000000000ULL) >> 40) | \ + (((_u64)(x) & (_u64)0xff00000000000000ULL) >> 56))) + + +#define __fast_swap(a, b) do { (a) ^= (b); (b) ^= (a); (a) ^= (b); } while(0) + + +static inline _u16 __byteswap_16(_u16 x) +{ +#ifdef __arch_byteswap_16 + return __arch_byteswap_16(x); +#else + return __static_byteswap_16(x); +#endif +} + +static inline _u32 __byteswap_32(_u32 x) +{ +#ifdef __arch_byteswap_32 + return __arch_byteswap_32(x); +#else + return __static_byteswap_32(x); +#endif +} + +static inline _u64 __byteswap_64(_u64 x) +{ +#ifdef __arch_byteswap_64 + return __arch_byteswap_64(x); +#else + return __static_byteswap_64(x); +#endif +} + + +#ifdef float +static inline float __byteswap_float(float x) +{ +#ifdef __arch_byteswap_float + return __arch_byteswap_float(x); +#else + _u8 * raw = (_u8 *)&x; + __fast_swap(raw[0], raw[3]); + __fast_swap(raw[1], raw[2]); + return x; +#endif +} +#endif + + +#ifdef double +static inline double __byteswap_double(double x) +{ +#ifdef __arch_byteswap_double + return __arch_byteswap_double(x); +#else + _u8 * raw = (_u8 *)&x; + __fast_swap(raw[0], raw[7]); + __fast_swap(raw[1], raw[6]); + __fast_swap(raw[2], raw[5]); + __fast_swap(raw[3], raw[4]); + return x; +#endif +} +#endif diff --git a/sdk/src/hal/byteorder.h b/sdk/src/hal/byteorder.h new file mode 100644 index 0000000..d06a9af --- /dev/null +++ b/sdk/src/hal/byteorder.h @@ -0,0 +1,112 @@ +/* + * RoboPeak Project + * Copyright 2009 - 2013 + * + * RPOS - Endianness Helper + * + */ + +#pragma once + + +#if !defined(_CPU_ENDIAN_BIG) && !defined(_CPU_ENDIAN_SMALL) +// CPU Endianness is not specified, assume little endian. +#define _CPU_ENDIAN_SMALL +#endif + +#if defined(_CPU_ENDIAN_BIG) && defined(_CPU_ENDIAN_SMALL) +#error "_CPU_ENDIAN_BIG and _CPU_ENDIAN_SMALL cannot be defined at the same time." +#endif + +#include "hal/byteops.h" + +#if defined(_CPU_ENDIAN_SMALL) + + +// we don't want to conflict with the Linux kernel... +#ifndef __KERNEL__ +#define constant_cpu_to_le64(x) ((_u64)(x)) +#define constant_le64_to_cpu(x) ((_u64)(x)) +#define constant_cpu_to_le32(x) ((_u32)(x)) +#define constant_le32_to_cpu(x) ((_u32)(x)) +#define constant_cpu_to_le16(x) ((_u16)(x)) +#define constant_le16_to_cpu(x) ((_u16)(x)) +#define constant_cpu_to_be64(x) (__static_byteswap_64((x))) +#define constant_be64_to_cpu(x) __static_byteswap_64((_u64)(x)) +#define constant_cpu_to_be32(x) (__static_byteswap_32((x))) +#define constant_be32_to_cpu(x) __static_byteswap_32((_u32)(x)) +#define constant_cpu_to_be16(x) (__static_byteswap_16((x))) +#define constant_be16_to_cpu(x) __static_byteswap_16((_u16)(x)) + +#define cpu_to_le64(x) ((_u64)(x)) +#define le64_to_cpu(x) ((_u64)(x)) +#define cpu_to_le32(x) ((_u32)(x)) +#define le32_to_cpu(x) ((_u32)(x)) +#define cpu_to_le16(x) ((_u16)(x)) +#define le16_to_cpu(x) ((_u16)(x)) +#define cpu_to_be64(x) (__byteswap_64((x))) +#define be64_to_cpu(x) __byteswap_64((_u64)(x)) +#define cpu_to_be32(x) (__byteswap_32((x))) +#define be32_to_cpu(x) __byteswap_32((_u32)(x)) +#define cpu_to_be16(x) (__byteswap_16((x))) +#define be16_to_cpu(x) __byteswap_16((_u16)(x)) +#endif + +#define cpu_to_float_le(x) ((float)x) +#define float_le_to_cpu(x) ((float)x) + +#define cpu_to_float_be(x) __byteswap_float(x) +#define float_be_to_cpu(x) __byteswap_float(x) + +#define cpu_to_double_le(x) ((double)x) +#define double_le_to_cpu(x) ((double)x) + +#define cpu_to_double_be(x) __byteswap_double(x) +#define double_be_to_cpu(x) __byteswap_double(x) + +#else + +// we don't want to conflict with the Linux kernel... +#ifndef __KERNEL__ +#define constant_cpu_to_le64(x) (__static_byteswap_64((x))) +#define constant_le64_to_cpu(x) __static_byteswap_64((_u64)(x)) +#define constant_cpu_to_le32(x) (__static_byteswap_32((x))) +#define constant_le32_to_cpu(x) __static_byteswap_32((_u32)(x)) +#define constant_cpu_to_le16(x) (__static_byteswap_16((x))) +#define constant_le16_to_cpu(x) __static_byteswap_16((_u16)(x)) +#define constant_cpu_to_be64(x) ((_u64)(x)) +#define constant_be64_to_cpu(x) ((_u64)(x)) +#define constant_cpu_to_be32(x) ((_u32)(x)) +#define constant_be32_to_cpu(x) ((_u32)(x)) +#define constant_cpu_to_be16(x) ((_u16)(x)) +#define constant_be16_to_cpu(x) ((_u16)(x)) + +#define cpu_to_le64(x) (__byteswap_64((x))) +#define le64_to_cpu(x) __byteswap_64((_u64)(x)) +#define cpu_to_le32(x) (__byteswap_32((x))) +#define le32_to_cpu(x) __byteswap_32((_u32)(x)) +#define cpu_to_le16(x) (__byteswap_16((x))) +#define le16_to_cpu(x) __byteswap_16((_u16)(x)) +#define cpu_to_be64(x) ((_u64)(x)) +#define be64_to_cpu(x) ((_u64)(x)) +#define cpu_to_be32(x) ((_u32)(x)) +#define be32_to_cpu(x) ((_u32)(x)) +#define cpu_to_be16(x) ((_u16)(x)) +#define be16_to_cpu(x) ((_u16)(x)) +#endif + + +#define cpu_to_float_le(x) __byteswap_float(x) +#define float_le_to_cpu(x) __byteswap_float(x) + +#define cpu_to_float_be(x) ((float)x) +#define float_be_to_cpu(x) ((float)x) + + +#define cpu_to_double_le(x) __byteswap_double(x) +#define double_le_to_cpu(x) __byteswap_double(x) + +#define cpu_to_double_be(x) ((double)x) +#define double_be_to_cpu(x) ((double)x) + +#endif diff --git a/sdk/src/hal/event.h b/sdk/src/hal/event.h new file mode 100644 index 0000000..ada15a5 --- /dev/null +++ b/sdk/src/hal/event.h @@ -0,0 +1,206 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +namespace rp{ namespace hal{ + +class Event +{ +public: + + enum + { + EVENT_OK = 1, + EVENT_TIMEOUT = 0xFFFFFFFF, + EVENT_FAILED = 0, + }; + + Event(bool isAutoReset = true, bool isSignal = false) +#ifdef _WIN32 + : _event(NULL) +#else + : _is_signalled(isSignal) + , _isAutoReset(isAutoReset) +#endif + { +#ifdef _WIN32 + _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); +#else + pthread_mutex_init(&_cond_locker, NULL); + pthread_condattr_init(&_cond_attr); +#ifdef _MACOS + // sadly, there is no monotonic clock support for pthread cond variable on MACOS + // if time slew is a big issue, try to reimplement it using kqueue/kevent +#else + pthread_condattr_setclock(&_cond_attr, CLOCK_MONOTONIC); +#endif + pthread_cond_init(&_cond_var, &_cond_attr); + +#endif + } + + ~ Event() + { + release(); + } + + void set( bool isSignal = true ) + { + if (isSignal){ +#ifdef _WIN32 + SetEvent(_event); +#else + pthread_mutex_lock(&_cond_locker); + + if ( _is_signalled == false ) + { + _is_signalled = true; + pthread_cond_signal(&_cond_var); + } + pthread_mutex_unlock(&_cond_locker); +#endif + } + else + { +#ifdef _WIN32 + ResetEvent(_event); +#else + pthread_mutex_lock(&_cond_locker); + _is_signalled = false; + pthread_mutex_unlock(&_cond_locker); +#endif + } + } + + unsigned long wait( unsigned long timeout = 0xFFFFFFFF ) + { +#ifdef _WIN32 + switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) + { + case WAIT_FAILED: + return EVENT_FAILED; + case WAIT_OBJECT_0: + return EVENT_OK; + case WAIT_TIMEOUT: + return EVENT_TIMEOUT; + } + return EVENT_OK; +#else + unsigned long ans = EVENT_OK; + pthread_mutex_lock( &_cond_locker ); + + if ( !_is_signalled ) + { + + if (timeout == 0xFFFFFFFF){ + pthread_cond_wait(&_cond_var,&_cond_locker); + }else + { + int timewaitresult = 0; +#ifdef _MACOS + timespec wait_time; + + wait_time.tv_sec = timeout / 1000; + wait_time.tv_nsec = (timeout%1000)*1000000ULL; + + timewaitresult = pthread_cond_timedwait_relative_np(&_cond_var,&_cond_locker,&wait_time); +#else + timespec wait_time; + clock_gettime(CLOCK_MONOTONIC, &wait_time); + + wait_time.tv_sec += timeout / 1000; + wait_time.tv_nsec += (timeout%1000)*1000000ULL; + + if (wait_time.tv_nsec >= 1000000000) + { + ++wait_time.tv_sec; + wait_time.tv_nsec -= 1000000000; + } + timewaitresult = pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time); +#endif + + switch (timewaitresult) + { + case 0: + // signalled + break; + case ETIMEDOUT: + // time up + ans = EVENT_TIMEOUT; + goto _final; + break; + default: + ans = EVENT_FAILED; + goto _final; + } + + } + } + + //assert(_is_signalled); + + if ( _isAutoReset ) + { + _is_signalled = false; + } +_final: + pthread_mutex_unlock( &_cond_locker ); + + return ans; +#endif + + } +protected: + + void release() + { +#ifdef _WIN32 + CloseHandle(_event); +#else + pthread_mutex_destroy(&_cond_locker); + pthread_cond_destroy(&_cond_var); +#endif + } + +#ifdef _WIN32 + HANDLE _event; +#else + pthread_cond_t _cond_var; + pthread_mutex_t _cond_locker; + pthread_condattr_t _cond_attr; + bool _is_signalled; + bool _isAutoReset; +#endif +}; +}} diff --git a/sdk/src/hal/locker.h b/sdk/src/hal/locker.h new file mode 100644 index 0000000..2a18e6b --- /dev/null +++ b/sdk/src/hal/locker.h @@ -0,0 +1,205 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +namespace rp{ namespace hal{ + +class Locker +{ +public: + enum LOCK_STATUS + { + LOCK_OK = 1, + LOCK_TIMEOUT = -1, + LOCK_FAILED = 0 + }; + + Locker(bool recusive = false){ +#ifdef _WIN32 + _lock = NULL; +#endif + init(recusive); + } + + ~Locker() + { + release(); + } + + Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF) + { +#ifdef _WIN32 + switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) + { + case WAIT_ABANDONED: + return LOCK_FAILED; + case WAIT_OBJECT_0: + return LOCK_OK; + case WAIT_TIMEOUT: + return LOCK_TIMEOUT; + } + +#else +#ifdef _MACOS + if (timeout !=0 ) { + if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; + } +#else + if (timeout == 0xFFFFFFFF){ + if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; + } +#endif + else if (timeout == 0) + { + if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK; + } +#ifndef _MACOS + else + { + timespec wait_time; + timeval now; + gettimeofday(&now,NULL); + + wait_time.tv_sec = timeout/1000 + now.tv_sec; + wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; + + if (wait_time.tv_nsec >= 1000000000) + { + ++wait_time.tv_sec; + wait_time.tv_nsec -= 1000000000; + } + switch (pthread_mutex_timedlock(&_lock,&wait_time)) + { + case 0: + return LOCK_OK; + case ETIMEDOUT: + return LOCK_TIMEOUT; + } + } +#endif +#endif + + return LOCK_FAILED; + } + + + void unlock() + { +#ifdef _WIN32 + if (_recusive) { + ReleaseMutex(_lock); + } else { + ReleaseSemaphore(_lock, 1, NULL); + } +#else + pthread_mutex_unlock(&_lock); +#endif + } + +#ifdef _WIN32 + HANDLE getLockHandle() + { + return _lock; + } +#else + pthread_mutex_t *getLockHandle() + { + return &_lock; + } +#endif + + + +protected: + void init(bool recusive) + { + +#ifdef _WIN32 + if (_recusive = recusive) { + _lock = CreateMutex(NULL, FALSE, NULL); + } else { + _lock = CreateSemaphore(NULL, 1, 1, NULL); + } +#else + + if (recusive) { + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&_lock, &attr); + } else { + pthread_mutex_init(&_lock, NULL); + } +#endif + } + + void release() + { + unlock(); //force unlock before release +#ifdef _WIN32 + + if (_lock) CloseHandle(_lock); + _lock = NULL; +#else + pthread_mutex_destroy(&_lock); +#endif + } + +#ifdef _WIN32 + HANDLE _lock; + bool _recusive; +#else + pthread_mutex_t _lock; +#endif + +}; + +class AutoLocker +{ +public : + AutoLocker(Locker &l): _binded(l) + { + _binded.lock(); + } + + void forceUnlock() { + _binded.unlock(); + } + ~AutoLocker() {_binded.unlock();} + Locker & _binded; +}; + + +}} + diff --git a/sdk/src/hal/socket.h b/sdk/src/hal/socket.h new file mode 100644 index 0000000..cf4fab0 --- /dev/null +++ b/sdk/src/hal/socket.h @@ -0,0 +1,149 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2009 - 2013 RoboPeak Project + */ + +#pragma once + +#include + +namespace rp{ namespace net { + +class _single_thread SocketAddress +{ + +public: + enum address_type_t { + ADDRESS_TYPE_UNSPEC = 0, + ADDRESS_TYPE_INET = 1, + ADDRESS_TYPE_INET6 = 2, + }; + +public: + + + + SocketAddress(); + SocketAddress(const char * addrString, int port, address_type_t = ADDRESS_TYPE_INET); + // do not use this function, internal usage + SocketAddress(void * platform_data); + SocketAddress(const SocketAddress &); + + SocketAddress & operator = (const SocketAddress &); + + virtual ~SocketAddress(); + + virtual int getPort() const; + virtual u_result setPort(int port); + + virtual u_result setAddressFromString(const char * address_string, address_type_t = ADDRESS_TYPE_INET); + virtual u_result getAddressAsString(char * buffer, size_t buffersize) const; + + virtual address_type_t getAddressType() const; + + virtual u_result getRawAddress(_u8 * buffer, size_t bufferSize) const; + + const void * getPlatformData() const { + return _platform_data; + } + + virtual void setLoopbackAddress(address_type_t = ADDRESS_TYPE_INET); + virtual void setBroadcastAddressIPv4(); + virtual void setAnyAddress(address_type_t = ADDRESS_TYPE_INET); + +public: + static size_t LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS = true, address_type_t = ADDRESS_TYPE_INET); + +protected: + void * _platform_data; +}; + + + +class SocketBase +{ +public: + enum socket_family_t { + SOCKET_FAMILY_INET = 0, + SOCKET_FAMILY_INET6 = 1, + SOCKET_FAMILY_RAW = 2, + }; + + + enum socket_direction_mask { + SOCKET_DIR_RD = 0x1, + SOCKET_DIR_WR = 0x2, + SOCKET_DIR_BOTH = (SOCKET_DIR_RD | SOCKET_DIR_WR), + }; + + enum { + DEFAULT_SOCKET_TIMEOUT = 10000, //10sec + }; + + virtual ~SocketBase() {} + virtual void dispose() = 0; + virtual u_result bind(const SocketAddress & ) = 0; + + virtual u_result getLocalAddress(SocketAddress & ) = 0; + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk = SOCKET_DIR_BOTH) = 0; + + virtual u_result waitforSent(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; + virtual u_result waitforData(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; +protected: + SocketBase() {} +}; + + +class _single_thread StreamSocket : public SocketBase +{ +public: + + enum { + MAX_BACKLOG = 128, + }; + + static StreamSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); + + virtual u_result connect(const SocketAddress & pairAddress) = 0; + + virtual u_result listen(int backlog = MAX_BACKLOG) = 0; + virtual StreamSocket * accept(SocketAddress * pairAddress = NULL) = 0; + virtual u_result waitforIncomingConnection(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; + + virtual u_result send(const void * buffer, size_t len) = 0; + + virtual u_result recv(void *buf, size_t len, size_t & recv_len) = 0; + + virtual u_result getPeerAddress(SocketAddress & ) = 0; + + virtual u_result shutdown(socket_direction_mask mask) = 0; + + virtual u_result enableKeepAlive(bool enable = true) = 0; + + virtual u_result enableNoDelay(bool enable = true) = 0; + +protected: + virtual ~StreamSocket() {} // use dispose(); + StreamSocket() {} +}; + +class _single_thread DGramSocket: public SocketBase +{ + +public: + + static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); + + virtual u_result setPairAddress(const SocketAddress * pairAddress) = 0; + virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len) = 0; + virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr = NULL) = 0; + virtual u_result clearRxCache() = 0; + +protected: + virtual ~DGramSocket() {} // use dispose(); + + DGramSocket() {} +}; + +}} diff --git a/sdk/src/hal/thread.cpp b/sdk/src/hal/thread.cpp new file mode 100644 index 0000000..bc68dd5 --- /dev/null +++ b/sdk/src/hal/thread.cpp @@ -0,0 +1,48 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "hal/thread.h" + +#if defined(_WIN32) +#include "arch/win32/winthread.hpp" +#elif defined(_MACOS) +#include "arch/macOS/thread.hpp" +#elif defined(__GNUC__) +#include "arch/linux/thread.hpp" +#else +#error no threading implemention found for this platform. +#endif + + diff --git a/sdk/src/hal/thread.h b/sdk/src/hal/thread.h new file mode 100644 index 0000000..74b6ff7 --- /dev/null +++ b/sdk/src/hal/thread.h @@ -0,0 +1,94 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "hal/types.h" +#define CLASS_THREAD(c , x ) \ + rp::hal::Thread::create_member(this ) + +namespace rp{ namespace hal{ + +class Thread +{ +public: + enum priority_val_t + { + PRIORITY_REALTIME = 0, + PRIORITY_HIGH = 1, + PRIORITY_NORMAL = 2, + PRIORITY_LOW = 3, + PRIORITY_IDLE = 4, + }; + + template + static Thread create_member(T * pthis) + { + return create(_thread_thunk, pthis); + } + + template + static _word_size_t THREAD_PROC _thread_thunk(void * data) + { + return (static_cast(data)->*PROC)(); + } + static Thread create(thread_proc_t proc, void * data = NULL ); + +public: + ~Thread() { } + Thread(): _data(NULL),_func(NULL),_handle(0) {} + _word_size_t getHandle(){ return _handle;} + u_result terminate(); + void *getData() { return _data;} + u_result join(unsigned long timeout = -1); + + // disabled as on platforms like Linux, the priority will be inherited by the child thread + // which may caused unexpected behavior. + // Please using Thread::SetSelfPriority instead + // u_result setPriority( priority_val_t p); + priority_val_t getPriority(); + + static u_result SetSelfPriority(priority_val_t p); + + + bool operator== ( const Thread & right) { return this->_handle == right._handle; } +protected: + Thread( thread_proc_t proc, void * data ): _data(data),_func(proc), _handle(0) {} + void * _data; + thread_proc_t _func; + _word_size_t _handle; +}; + +}} + diff --git a/sdk/src/hal/types.h b/sdk/src/hal/types.h new file mode 100644 index 0000000..6e23faa --- /dev/null +++ b/sdk/src/hal/types.h @@ -0,0 +1,119 @@ +/* + * Common Data Types for RP + */ + +#ifndef _INFRA_HAL_TYPES_H_ +#define _INFRA_HAL_TYPES_H_ + +//Basic types +// +#ifdef WIN32 + +//fake stdint.h for VC only + +typedef signed char int8_t; +typedef unsigned char uint8_t; + +typedef __int16 int16_t; +typedef unsigned __int16 uint16_t; + +typedef __int32 int32_t; +typedef unsigned __int32 uint32_t; + +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; + + +#define RPMODULE_EXPORT __declspec(dllexport) +#define RPMODULE_IMPORT __declspec(dllimport) + +#else + +#include + +#define RPMODULE_EXPORT +#define RPMODULE_IMPORT + +#endif + + +//based on stdint.h +typedef int8_t _s8; +typedef uint8_t _u8; + +typedef int16_t _s16; +typedef uint16_t _u16; + +typedef int32_t _s32; +typedef uint32_t _u32; + +typedef int64_t _s64; +typedef uint64_t _u64; + +#define __small_endian + +#ifndef __GNUC__ +#define __attribute__(x) +#endif + + +// The _word_size_t uses actual data bus width of the current CPU +#ifdef _AVR_ +typedef _u8 _word_size_t; +#define THREAD_PROC +#elif defined (WIN64) +typedef _u64 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (WIN32) +typedef size_t _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (__GNUC__) +typedef unsigned long _word_size_t; +#define THREAD_PROC +#elif defined (__ICCARM__) +typedef _u32 _word_size_t; +#define THREAD_PROC +#endif + + + +//#define __le +//#define __be + +#define _multi_thread +#define _single_thread + +typedef uint32_t u_result; + +#define RESULT_OK 0 +#define RESULT_FAIL_BIT 0x80000000 +#define RESULT_ALREADY_DONE 0x20 +#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) +#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) +#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_ABORTED (0x8007 | RESULT_FAIL_BIT) +#define RESULT_NOT_FOUND (0x8008 | RESULT_FAIL_BIT) +#define RESULT_RECONNECTING (0x8009 | RESULT_FAIL_BIT) + +#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) +#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) + + +typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); + + +#if defined (_BUILD_AS_DLL) +#if defined (_BUILD_DLL_EXPORT) +#define RPMODULE_IMPEXP RPMODULE_EXPORT +#else +#define RPMODULE_IMPEXP RPMODULE_IMPORT +#endif +#else +#define RPMODULE_IMPEXP +#endif + +#endif diff --git a/sdk/src/hal/util.h b/sdk/src/hal/util.h new file mode 100644 index 0000000..205a858 --- /dev/null +++ b/sdk/src/hal/util.h @@ -0,0 +1,67 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + + +//------ +/* _countof helper */ +#if !defined(_countof) +#if !defined(__cplusplus) +#define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0])) +#else +extern "C++" +{ +template +char (*__countof_helper( _CountofType (&_Array)[_SizeOfArray]))[_SizeOfArray]; +#define _countof(_Array) sizeof(*__countof_helper(_Array)) +} +#endif +#endif + +/* _offsetof helper */ +#if !defined(offsetof) +#define offsetof(_structure, _field) ((_word_size_t)&(((_structure *)0x0)->_field)) +#endif + + +#define BEGIN_STATIC_CODE( _blockname_ ) \ + static class _static_code_##_blockname_ { \ + public: \ + _static_code_##_blockname_ () + + +#define END_STATIC_CODE( _blockname_ ) \ + } _instance_##_blockname_; + diff --git a/sdk/src/hal/waiter.h b/sdk/src/hal/waiter.h new file mode 100644 index 0000000..7f83b9b --- /dev/null +++ b/sdk/src/hal/waiter.h @@ -0,0 +1,44 @@ +/* + * For synchronize asynchrous operations + * + * Copyright 2010 Robopeak Team + */ +#pragma once + +#ifdef _AVR_ +#error there is no implementation for waiter.h on AVR platforms +#else + +#include "hal/event.h" + +namespace rp{ namespace hal{ + + template + class Waiter : public Event + { + public: + Waiter() + : Event() + { + } + + ~Waiter() + {} + + ResultT waitForResult() + { + wait(); + return result; + } + + void setResult(ResultT result) + { + this->result = result; + set(); + } + + volatile ResultT result; + }; +}} + +#endif diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp new file mode 100644 index 0000000..beedd39 --- /dev/null +++ b/sdk/src/rplidar_driver.cpp @@ -0,0 +1,199 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "rplidar_driver.h" +#include "sl_crc.h" +#include + +namespace rp { namespace standalone{ namespace rplidar { + + RPlidarDriver::RPlidarDriver(){} + + RPlidarDriver::RPlidarDriver(sl_u32 channelType) + :_channelType(channelType) + { + } + + RPlidarDriver::~RPlidarDriver() {} + + RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) + { + //_channelType = drivertype; + return new RPlidarDriver(drivertype); + } + + void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) + { + delete drv; + } + + u_result RPlidarDriver::connect(const char *path, _u32 portOrBaud, _u32 flag) + { + switch (_channelType) + { + case CHANNEL_TYPE_SERIALPORT: + _channel = (*createSerialPortChannel(path, portOrBaud)); + break; + case CHANNEL_TYPE_TCP: + _channel = *createTcpChannel(path, portOrBaud); + break; + case CHANNEL_TYPE_UDP: + _channel = *createUdpChannel(path, portOrBaud); + break; + } + if (!(bool)_channel) return SL_RESULT_OPERATION_FAIL; + + _lidarDrv = *createLidarDriver(); + + if (!(bool)_lidarDrv) return SL_RESULT_OPERATION_FAIL; + + sl_result ans =(_lidarDrv)->connect(_channel); + return ans; + } + + void RPlidarDriver::disconnect() + { + (_lidarDrv)->disconnect(); + } + + bool RPlidarDriver::isConnected() + { + return (_lidarDrv)->isConnected(); + } + + u_result RPlidarDriver::reset(_u32 timeout) + { + return (_lidarDrv)->reset(); + } + + u_result RPlidarDriver::getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs) + { + return (_lidarDrv)->getAllSupportedScanModes(outModes, timeoutInMs); + } + + u_result RPlidarDriver::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs) + { + return (_lidarDrv)->getTypicalScanMode(outMode, timeoutInMs); + } + + u_result RPlidarDriver::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) + { + return (_lidarDrv)->startScan(force, useTypicalScan, options, outUsedScanMode); + } + + u_result RPlidarDriver::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout) + { + return (_lidarDrv)->startScanExpress(force, scanMode, options, outUsedScanMode, timeout); + } + + u_result RPlidarDriver::getHealth(rplidar_response_device_health_t & health, _u32 timeout) + { + return (_lidarDrv)->getHealth(health, timeout); + } + + u_result RPlidarDriver::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) + { + return (_lidarDrv)->getDeviceInfo(info, timeout); + } + + u_result RPlidarDriver::setMotorPWM(_u16 pwm) + { + return (_lidarDrv)->setMotorSpeed(pwm); + } + + u_result RPlidarDriver::checkMotorCtrlSupport(bool & support, _u32 timeout) + { + MotorCtrlSupport motorSupport; + u_result ans = (_lidarDrv)->checkMotorCtrlSupport(motorSupport, timeout); + if (motorSupport == MotorCtrlSupportNone) + support = false; + return ans; + } + + u_result RPlidarDriver::setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout) + { + return (_lidarDrv)->setLidarIpConf(conf, timeout); + } + + u_result RPlidarDriver::getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout) + { + return (_lidarDrv)->getLidarIpConf(conf, timeout); + } + + u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs) + { + return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs); + } + + u_result RPlidarDriver::stop(_u32 timeout) + { + return (_lidarDrv)->stop(timeout); + } + + u_result RPlidarDriver::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout) + { + return (_lidarDrv)->grabScanDataHq(nodebuffer, count, timeout); + } + + u_result RPlidarDriver::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) + { + return (_lidarDrv)->ascendScanData(nodebuffer, count); + } + + u_result RPlidarDriver::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) + { + return RESULT_OPERATION_NOT_SUPPORT; + } + + u_result RPlidarDriver::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) + { + return (_lidarDrv)->getScanDataWithIntervalHq(nodebuffer, count); + } + + u_result RPlidarDriver::startMotor() + { + return (_lidarDrv)->setMotorSpeed(DEFAULT_MOTOR_SPEED); + } + u_result RPlidarDriver::stopMotor() + { + return (_lidarDrv)->setMotorSpeed(0); + } + +}}} diff --git a/sdk/src/sdkcommon.h b/sdk/src/sdkcommon.h new file mode 100644 index 0000000..f928b42 --- /dev/null +++ b/sdk/src/sdkcommon.h @@ -0,0 +1,51 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#if defined(_WIN32) + +#include "arch/win32/arch_win32.h" +#elif defined(_MACOS) +#include "arch/macOS/arch_macOS.h" +#elif defined(__GNUC__) +#include "arch/linux/arch_linux.h" +#else +#error "unsupported target" +#endif + +#include "hal/types.h" +#include "hal/assert.h" + +#include "rplidar.h" + +#include "hal/util.h" \ No newline at end of file diff --git a/sdk/src/sl_async_transceiver.cpp b/sdk/src/sl_async_transceiver.cpp new file mode 100644 index 0000000..c8033a5 --- /dev/null +++ b/sdk/src/sl_async_transceiver.cpp @@ -0,0 +1,412 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + + + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" + +#include "sl_async_transceiver.h" + + + +namespace sl { namespace internal { + + + + +ProtocolMessage::ProtocolMessage() + : len(0) + , cmd(0) + , data(NULL) + , _databufsize(0) + , _usingOutterData(false) +{ + _changeBufSize(); +} + +ProtocolMessage::ProtocolMessage(_u8 cmd, const void* buffer, size_t size) + : len(size) + , cmd(cmd) + , data(NULL) + , _databufsize(0) + , _usingOutterData(false) +{ + _changeBufSize(); + if (buffer) + { + memcpy(data, buffer, size); + } +} + +ProtocolMessage::ProtocolMessage(const ProtocolMessage& srcMsg) + : len(srcMsg.len) + , cmd(srcMsg.cmd) + , data(NULL) + , _databufsize(0) + , _usingOutterData(false) +{ + _changeBufSize( true ); + if (srcMsg.data && len) + { + memcpy(data, srcMsg.data, len); + } +} + +ProtocolMessage::~ProtocolMessage() +{ + this->cleanData(); +} + +ProtocolMessage& ProtocolMessage::operator =(const ProtocolMessage& srcMessage) +{ + this->cleanData(); + + + this->len = srcMessage.len; + this->cmd = srcMessage.cmd; + + _changeBufSize( true ); + if (srcMessage.data && len) + { + memcpy(data, srcMessage.data, len); + } + + return *this; +} + +void ProtocolMessage::setDataBuf(_u8 *buffer, size_t size) +{ + this->cleanData(); + + len = size; + data = buffer; + _databufsize = size; + _usingOutterData = true; +} + +void ProtocolMessage::fillData(const void * buffer, size_t size) +{ + len = size; + _changeBufSize(); + if (buffer) + memcpy(data, buffer, size); +} + +void ProtocolMessage::cleanData() +{ + if (data) + { + if (!_usingOutterData) + { + delete [] data; + } + data = NULL; + len = 1; + _databufsize = 0; + } +} + +void ProtocolMessage::_changeBufSize( bool force_compact) +{ + size_t actual_size = getPayloadSize(); + + size_t new_buf_size = actual_size; + + + if (!_usingOutterData) + { + // nothing to do + if ( new_buf_size == _databufsize ) return; + + if ( new_buf_size < _databufsize){ + + if ( (_databufsize >> 1) < new_buf_size) + { + // reuse the current buffer + if (!force_compact) return; + }else + { + // the current buffer size is much bigger, we need to release it to save memory + } + } + } + + // we need to change the buffer + cleanData(); + // the cleanData() will reset the length info, so we need to restore it + len = actual_size; + data = new _u8[new_buf_size]; + _databufsize = new_buf_size; +} + + +AsyncTransceiver::AsyncTransceiver(IAsyncProtocolCodec& codec) + : _bindedChannel(NULL) + , _codec(codec) + , _isWorking(false) + , _workingFlag(0) +{ + +} + +AsyncTransceiver::~AsyncTransceiver() +{ + unbindAndClose(); +} + +u_result AsyncTransceiver::openChannelAndBind(IChannel* channel) +{ + if (!channel) return RESULT_INVALID_DATA; + + unbindAndClose(); + u_result ans = RESULT_OK; + do + { + rp::hal::AutoLocker l(_opLocker); + + // try to open the channel ... + Result ans = SL_RESULT_OK; + + if (!channel->open()) { + ans= RESULT_OPERATION_FAIL; + break; + } + + + // force a flush to clear any pending data + channel->flush(); + + _dataEvt.set(false); + + _isWorking = true; + _workingFlag = 0; + _bindedChannel = channel; + + + _decoderThread = CLASS_THREAD(AsyncTransceiver, _proc_decoderThread); + _rxThread = CLASS_THREAD(AsyncTransceiver, _proc_rxThread); + + + + + } while (0); + + return ans; +} + +void AsyncTransceiver::unbindAndClose() +{ + rp::hal::AutoLocker l(_opLocker); + if (!_isWorking) return; + + assert(_bindedChannel); + + + _isWorking = false; + _dataEvt.set(); // set signal to wake up threads + + _decoderThread.join(); + _rxThread.join(); + + + _bindedChannel->close(); + + _bindedChannel = NULL; + + + for (std::list< Buffer* >::iterator itr = _rxQueue.begin(); itr != _rxQueue.end(); ++itr) + { + delete [] *itr; + } + _rxQueue.clear(); + +} + +u_result AsyncTransceiver::sendMessage(message_autoptr_t& msg) +{ + assert(msg); + + if (!_isWorking) return RESULT_OPERATION_NOT_SUPPORT; + + rp::hal::AutoLocker l(_opLocker); + + size_t requiredBufferSize = _codec.estimateLength(msg); + + if (requiredBufferSize == 0) { + // nothing to send + return RESULT_OK; + } + + u_result ans = RESULT_OK; + + _u8* txBuffer = new _u8[requiredBufferSize]; + + do { + + if (!txBuffer) { + return RESULT_INSUFFICIENT_MEMORY; + } + + _codec.onEncodeData(msg, txBuffer, &requiredBufferSize); + + int txSize = _bindedChannel->write(txBuffer, requiredBufferSize); + + if (txSize < 0) ans = RESULT_OPERATION_FAIL; + + } while (0); + + + delete[] txBuffer; + return ans; +} + +sl_result AsyncTransceiver::_proc_rxThread() +{ + assert(_bindedChannel); + + rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH); + + u_result result; + size_t hintedSize = 0; + while (_isWorking) + { + result = _bindedChannel->waitForDataExt(hintedSize, 1000); + + if (IS_FAIL(result)) + { + // timeout is allowed + if (result == RESULT_OPERATION_TIMEOUT) { + continue; + } + if (_isWorking) { + _workingFlag |= WORKING_FLAG_ERROR; + _codec.onChannelError(result); + break; + } + } + + // no data in buffer, sleep and wait for the next round + if (!hintedSize) + { + continue; + } + + + Buffer* decodeBuffer = new Buffer(); + + decodeBuffer->data = new _u8[hintedSize]; + + decodeBuffer->size = _bindedChannel->read(decodeBuffer->data, hintedSize); +#ifdef _DEBUG_DUMP_PACKET + printf("Revc: %d\n", decodeBuffer->size); +#endif + + if (!decodeBuffer->size) { + delete decodeBuffer; + + + _workingFlag |= WORKING_FLAG_ERROR; + _codec.onChannelError(RESULT_OPERATION_ABORTED); + break; + } + + assert(hintedSize >= decodeBuffer->size); + + +#ifdef _DEBUG_DUMP_PACKET + printf("=== Dump RX Packet, size = %d ===\n", decodeBuffer->size); + for (int pos = 0; pos < decodeBuffer->size; pos++) + { + printf("%02x ", decodeBuffer->data[pos]); + } + printf("\n=== END ===\n"); +#endif + + _rxLocker.lock(); + _rxQueue.push_back(decodeBuffer); + _dataEvt.set(); + _rxLocker.unlock(); + + + } + _workingFlag |= WORKING_FLAG_RX_DISABLED; + return RESULT_OK; +} + +sl_result AsyncTransceiver::_proc_decoderThread() +{ + + assert(_bindedChannel); + rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH); + _codec.onDecodeReset(); + + + while (_isWorking) + { + _rxLocker.lock(); + + if (_rxQueue.empty()) + { + _rxLocker.unlock(); + + if (_dataEvt.wait(1000)) + continue; + + _rxLocker.lock(); + } + assert(!_rxQueue.empty()); + + Buffer * bufferToDecode = _rxQueue.front(); + _rxQueue.pop_front(); + + _rxLocker.unlock(); + + //cout<<"decoding "<< bufferToDecode->size <<" bytes of data"<data, bufferToDecode->size); + + + delete bufferToDecode; + } + + return RESULT_OK; + +} + + +}} \ No newline at end of file diff --git a/sdk/src/sl_async_transceiver.h b/sdk/src/sl_async_transceiver.h new file mode 100644 index 0000000..75774e8 --- /dev/null +++ b/sdk/src/sl_async_transceiver.h @@ -0,0 +1,167 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace sl { namespace internal { + + +class _single_thread ProtocolMessage { + +public: + size_t len; + _u8 cmd; +protected: + _u8* data; + size_t _databufsize; + +public: + ProtocolMessage(); + ProtocolMessage(_u8 cmd, const void* buffer, size_t size); + ProtocolMessage(const ProtocolMessage& srcMsg); + virtual ~ProtocolMessage(); + + ProtocolMessage& operator=(const ProtocolMessage& srcMessage); + + // avoid use this method, pls. use fillData instead + void setDataBuf(_u8* buffer, size_t size); + + _u8* getDataBuf() { return data; } + + void fillData(const void* buffer, size_t size); + void cleanData(); + + size_t getPayloadSize() const + { + return len; + } + +protected: + + // change the data buffer to fix the new payload size + // the existing buffer will be reused if possible. + // all the existing payload data will lose + void _changeBufSize(bool force_compact = false); + bool _usingOutterData; +}; + + + +typedef std::shared_ptr message_autoptr_t; + + +class IAsyncProtocolCodec { +public: + IAsyncProtocolCodec() {} + virtual ~IAsyncProtocolCodec() {} + + virtual void onChannelError(u_result errCode) {} + + virtual void onDecodeReset() {} + virtual void onDecodeData(const void* buffer, size_t size) = 0; + + + virtual size_t estimateLength(message_autoptr_t& message) = 0; + virtual void onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size) = 0; + +}; + +class AsyncTransceiver { +public: + + enum working_flag_t + { + WORKING_FLAG_RX_DISABLED = 0x1L << 0, + WORKING_FLAG_TX_DISABLED = 0x1L << 1, + + WORKING_FLAG_ERROR = 0x1L << 31, + }; + + + AsyncTransceiver(IAsyncProtocolCodec& codec); + ~AsyncTransceiver(); + + + + u_result openChannelAndBind(IChannel* channel); + void unbindAndClose(); + + IChannel* getBindedChannel() const { + return _bindedChannel; + } + + u_result sendMessage(message_autoptr_t& msg); + +protected: + + sl_result _proc_rxThread(); + sl_result _proc_decoderThread(); + +protected: + + + rp::hal::Locker _opLocker; + rp::hal::Locker _rxLocker; + rp::hal::Event _dataEvt; + + IChannel* _bindedChannel; + IAsyncProtocolCodec& _codec; + + + bool _isWorking; + _u32 _workingFlag; + + rp::hal::Thread _rxThread; + rp::hal::Thread _decoderThread; + + struct Buffer { + size_t size; + _u8* data; + + + Buffer() : size(0), data(NULL){} + + ~Buffer() { + if (data) { + delete[] data; + data = NULL; + } + } + }; + std::list< Buffer * > _rxQueue; +}; + + +}} diff --git a/sdk/src/sl_crc.cpp b/sdk/src/sl_crc.cpp new file mode 100644 index 0000000..49d7844 --- /dev/null +++ b/sdk/src/sl_crc.cpp @@ -0,0 +1,102 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sl_crc.h" + +namespace sl {namespace crc32 { + + static sl_u32 table[256];//crc32_table + sl_u32 bitrev(sl_u32 input, sl_u16 bw) + { + sl_u16 i; + sl_u32 var; + var = 0; + for (i = 0; i < bw; i++) { + if (input & 0x01) { + var |= 1 << (bw - 1 - i); + } + input >>= 1; + } + return var; + } + + void init(sl_u32 poly) + { + sl_u16 i; + sl_u16 j; + sl_u32 c; + + poly = bitrev(poly, 32); + for (i = 0; i < 256; i++) { + c = i; + for (j = 0; j < 8; j++) { + if (c & 1) + c = poly ^ (c >> 1); + else + c = c >> 1; + } + table[i] = c; + } + } + + sl_u32 cal(sl_u32 crc, void* input, sl_u16 len) + { + sl_u16 i; + sl_u8 index; + sl_u8* pch; + pch = (unsigned char*)input; + sl_u8 leftBytes = 4 - (len & 0x3); + + for (i = 0; i < len; i++) { + index = (unsigned char)(crc^*pch); + crc = (crc >> 8) ^ table[index]; + pch++; + } + + for (i = 0; i < leftBytes; i++) {//zero padding + index = (unsigned char)(crc ^ 0); + crc = (crc >> 8) ^ table[index]; + } + return crc ^ 0xffffffff; + } + + sl_result getResult(sl_u8 *ptr, sl_u32 len) + { + static sl_u8 tmp; + if (tmp != 1) { + init(0x4C11DB7); + tmp = 1; + } + + return cal(0xFFFFFFFF, ptr, len); + } +}} \ No newline at end of file diff --git a/sdk/src/sl_lidar_driver.cpp b/sdk/src/sl_lidar_driver.cpp new file mode 100644 index 0000000..4fd719c --- /dev/null +++ b/sdk/src/sl_lidar_driver.cpp @@ -0,0 +1,1702 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "hal/waiter.h" +#include "hal/byteorder.h" +#include "sl_lidar_driver.h" +#include "sl_crc.h" +#include +#include +#include +#include + +#include "dataunpacker/dataunpacker.h" +#include "sl_async_transceiver.h" +#include "sl_lidarprotocol_codec.h" + + + +#ifdef _WIN32 +#define NOMINMAX +#undef min +#undef max +#endif + +#if defined(__cplusplus) && __cplusplus >= 201103L +#ifndef _GXX_NULLPTR_T +#define _GXX_NULLPTR_T +typedef decltype(nullptr) nullptr_t; +#endif +#endif /* C++11. */ + +namespace sl { + static void printDeprecationWarn(const char* fn, const char* replacement) + { + fprintf(stderr, "*WARN* YOU ARE USING DEPRECATED API: %s, PLEASE MOVE TO %s\n", fn, replacement); + } + + static void convert(const sl_lidar_response_measurement_node_t& from, sl_lidar_response_measurement_node_hq_t& to) + { + to.angle_z_q14 = (((from.angle_q6_checkbit) >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle + to.dist_mm_q2 = from.distance_q2; + to.flag = (from.sync_quality & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field + to.quality = (from.sync_quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 + } + + static void convert(const sl_lidar_response_measurement_node_hq_t& from, sl_lidar_response_measurement_node_t& to) + { + to.sync_quality = (from.flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) | ((from.quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + to.angle_q6_checkbit = 1 | (((from.angle_z_q14 * 90) >> 8) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT); + to.distance_q2 = from.dist_mm_q2 > sl_u16(-1) ? sl_u16(0) : sl_u16(from.dist_mm_q2); + } + + + static inline float getAngle(const sl_lidar_response_measurement_node_t& node) + { + return (node.angle_q6_checkbit >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.f; + } + + static inline void setAngle(sl_lidar_response_measurement_node_t& node, float v) + { + sl_u16 checkbit = node.angle_q6_checkbit & SL_LIDAR_RESP_MEASUREMENT_CHECKBIT; + node.angle_q6_checkbit = (((sl_u16)(v * 64.0f)) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) | checkbit; + } + + static inline float getAngle(const sl_lidar_response_measurement_node_hq_t& node) + { + return node.angle_z_q14 * 90.f / 16384.f; + } + + static inline void setAngle(sl_lidar_response_measurement_node_hq_t& node, float v) + { + node.angle_z_q14 = sl_u32(v * 16384.f / 90.f); + } + + static inline sl_u16 getDistanceQ2(const sl_lidar_response_measurement_node_t& node) + { + return node.distance_q2; + } + + static inline sl_u32 getDistanceQ2(const sl_lidar_response_measurement_node_hq_t& node) + { + return node.dist_mm_q2; + } + + template + static bool angleLessThan(const TNode& a, const TNode& b) + { + return getAngle(a) < getAngle(b); + } + + template < class TNode > + static sl_result ascendScanData_(TNode * nodebuffer, size_t count) + { + float inc_origin_angle = 360.f / count; + size_t i = 0; + + //Tune head + for (i = 0; i < count; i++) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } + else { + while (i != 0) { + i--; + float expect_angle = getAngle(nodebuffer[i + 1]) - inc_origin_angle; + if (expect_angle < 0.0f) expect_angle = 0.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + // all the data is invalid + if (i == count) return SL_RESULT_OPERATION_FAIL; + + //Tune tail + for (i = count - 1; i < count; i--) { + // To avoid array overruns, use the i < count condition + if (getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } + else { + while (i != (count - 1)) { + i++; + float expect_angle = getAngle(nodebuffer[i - 1]) + inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + //Fill invalid angle in the scan + float frontAngle = getAngle(nodebuffer[0]); + for (i = 1; i < count; i++) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + } + + // Reorder the scan according to the angle value + std::sort(nodebuffer, nodebuffer + count, &angleLessThan); + + return SL_RESULT_OK; + } + + template + class RawSampleNodeHolder + { + public: + RawSampleNodeHolder(size_t maxcount = 8192) + : _max_count(maxcount) + { + + } + void clear() + { + rp::hal::AutoLocker l(_locker); + _data_waiter.set(false); + _data_queue.clear(); + } + + void pushNode(_u64 timestamp_uS, const T* node) + { + rp::hal::AutoLocker l(_locker); + _data_queue.push_back(*node); + if (_data_queue.size() > _max_count) { + _data_queue.pop_front(); + } + _data_waiter.set(); + } + + size_t waitAndFetch(T* node, size_t maxcount, _u32 timeout) + { + if (_data_waiter.wait(timeout) == rp::hal::Event::EVENT_OK) + { + rp::hal::AutoLocker l(_locker); + + size_t copiedCount = 0; + + while (maxcount--) { + node[copiedCount++] = _data_queue.front(); + _data_queue.pop_front(); + } + + return copiedCount; + } + return 0; + } + protected: + size_t _max_count; + rp::hal::Locker _locker; + rp::hal::Event _data_waiter; + std::deque _data_queue; + + }; + + template + class ScanDataHolder + { + public: + ScanDataHolder(size_t maxcount = 8192) + : _scan_node_buffer_size(maxcount) + , _scan_node_available_id(-1) + , _new_scan_ready(false) + { + _scanbuffer[0].reserve(_scan_node_buffer_size); + _scanbuffer[1].reserve(_scan_node_buffer_size); + + memset(_scan_begin_timestamp_uS, 0, sizeof(_scan_begin_timestamp_uS)); + } + + size_t getMaxCacheCount() const { + return _scan_node_buffer_size; + } + + + void reset() { + rp::hal::AutoLocker l(_locker); + _scan_node_available_id = -1; + _new_scan_ready = false; + _scanbuffer[0].clear(); + _scanbuffer[1].clear(); + _data_waiter.set(false); + memset(_scan_begin_timestamp_uS, 0, sizeof(_scan_begin_timestamp_uS)); + } + + bool checkNewScanSignalAndReset() + { + return _new_scan_ready.exchange(false); + } + + void pushScanNodeData(_u64 currentSampleTsUs, const T* hqNode) + { + rp::hal::AutoLocker l(_locker); + + int operationBufID = _getOperationBufferID_locked(); + auto operationalBuf = &_scanbuffer[operationBufID]; + + if (hqNode->flag & RPLIDAR_RESP_HQ_FLAG_SYNCBIT) { + if (operationalBuf->size()) { + operationBufID = _finishCurrentScanAndSwap_locked(); + operationalBuf = &_scanbuffer[operationBufID]; + + // publish the available scan + _new_scan_ready = true; + _data_waiter.set(); + + } + + assert(operationalBuf->size() == 0); + + //store the timestamp info + _scan_begin_timestamp_uS[operationBufID] = currentSampleTsUs; + } + else { + if (operationalBuf->size() == 0) { + //discard the data, do not form partial scan + return; + } + } + + if (operationalBuf->size() >= _scan_node_buffer_size) { + //replace the last entry if buffer is full + operationalBuf->at(operationalBuf->size() - 1) = *hqNode; + } + else { + operationalBuf->push_back(*hqNode); + } + + } + + void rewindCurrentScanData() { + rp::hal::AutoLocker l(_locker); + _getOperationalBuffer_locked().clear(); + } + + std::vector* waitAndLockAvailableScan(_u32 timeout, _u64 * out_timestamp_uS = nullptr) + { + if (_data_waiter.wait(timeout) == rp::hal::Event::EVENT_OK) + { + _locker.lock(); + assert(_scan_node_available_id >= 0); + _new_scan_ready = false; + if (out_timestamp_uS) { + *out_timestamp_uS = _scan_begin_timestamp_uS[_scan_node_available_id]; + } + return &_scanbuffer[_scan_node_available_id]; + } + else { + return nullptr; + } + } + + void unlockScan(std::vector* scan) { + if (scan) { + _locker.unlock(); + } + } + + protected: + int _finishCurrentScanAndSwap_locked() { + _scan_node_available_id = _getOperationBufferID_locked(); + int newOperationalID = 1 - _scan_node_available_id; + + _scanbuffer[newOperationalID].clear(); + return newOperationalID; + } + + int _getOperationBufferID_locked() { + if (_scan_node_available_id < 0) return 0; + return 1 - _scan_node_available_id; + } + + std::vector& _getOperationalBuffer_locked() + { + return _scanbuffer[_getOperationBufferID_locked()]; + } + + + rp::hal::Locker _locker; + rp::hal::Event _data_waiter; + + + + _u64 _scan_begin_timestamp_uS[2]; + size_t _scan_node_buffer_size; + int _scan_node_available_id; + std::atomic _new_scan_ready; + + std::vector _scanbuffer[2]; + }; + + class SlamtecLidarDriver : + public ILidarDriver, internal::IProtocolMessageListener, internal::LIDARSampleDataListener + { + public: + enum { + MAX_SCANNODE_CACHE_COUNT = 8192, + }; + + enum { + A2A3_LIDAR_MINUM_MAJOR_ID = 2, + BUILTIN_MOTORCTL_MINUM_MAJOR_ID = 6, + }; + + + enum { + TOF_C_SERIAL_MINUM_MAJOR_ID = 4, + TOF_S_SERIAL_MINUM_MAJOR_ID = 6, + TOF_T_SERIAL_MINUM_MAJOR_ID = 9, + TOF_M_SERIAL_MINUM_MAJOR_ID = 12, + + + NEWDESIGN_MINUM_MAJOR_ID = TOF_C_SERIAL_MINUM_MAJOR_ID, + }; + + public: + SlamtecLidarDriver() + : _isConnected(false) + , _isSupportingMotorCtrl(MotorCtrlSupportNone) + , _op_locker(true) + , _scanHolder(MAX_SCANNODE_CACHE_COUNT) + , _rawSampleNodeHolder(MAX_SCANNODE_CACHE_COUNT) + , _waiting_packet_type(0) + { + _protocolHandler = std::make_shared< internal::RPLidarProtocolCodec>(); + _transeiver = std::make_shared< internal::AsyncTransceiver>(*_protocolHandler); + _dataunpacker.reset(internal::LIDARSampleDataUnpacker::CreateInstance(*this)); + + _protocolHandler->setMessageListener(this); + + memset(&_cached_DevInfo, 0, sizeof(_cached_DevInfo)); + } + + + virtual ~SlamtecLidarDriver() + { + disconnect(); + _protocolHandler->setMessageListener(nullptr); + } + + + LIDARTechnologyType getLIDARTechnologyType(const sl_lidar_response_device_info_t* devInfo) + { + rp::hal::AutoLocker l(_op_locker); + if (!devInfo) { + devInfo = &_cached_DevInfo; + } + + return ParseLIDARTechnologyTypeByModelID(devInfo->model); + } + + LIDARMajorType getLIDARMajorType(const sl_lidar_response_device_info_t* devInfo) + { + rp::hal::AutoLocker l(_op_locker); + if (!devInfo) { + devInfo = &_cached_DevInfo; + } + + return ParseLIDARMajorTypeByModelID(devInfo->model); + + } + + sl_result getModelNameDescriptionString(std::string& out_description, bool fetchAliasName, const sl_lidar_response_device_info_t* devInfo, sl_u32 timeout) + { + rp::hal::AutoLocker l(_op_locker); + u_result ans; + // fetch alias (commerical) name if asked: + if (fetchAliasName) { + std::vector<_u8> replyData; + ans = getLidarConf(SL_LIDAR_CONF_MODEL_NAME_ALIAS, replyData, nullptr, 0, timeout); + if (IS_OK(ans) && replyData.size()) { + out_description.resize(replyData.size() + 1); + memcpy(&out_description[0], &replyData[0], replyData.size()); + out_description[replyData.size()] = '\0'; + if (out_description != "") { + return SL_RESULT_OK; + } + } + } + + + if (!devInfo) { + devInfo = &_cached_DevInfo; + } + + + out_description = GetModelNameStringByModelID(devInfo->model); + + return SL_RESULT_OK; + } + + sl_result connect(IChannel* channel) + { + rp::hal::AutoLocker l(_op_locker); + if (!channel) return SL_RESULT_OPERATION_FAIL; + if (isConnected()) return SL_RESULT_ALREADY_DONE; + + _rawSampleNodeHolder.clear(); + + sl_result ans; + + + ans = (sl_result)_transeiver->openChannelAndBind(channel); + + if (IS_OK(ans)) { + _isConnected = true; + // the first dev info local cache will be taken here + checkMotorCtrlSupport(_isSupportingMotorCtrl, 500); + } + + return ans; + } + + void disconnect() + { + rp::hal::AutoLocker l(_op_locker); + if (_isConnected) { + _disableDataGrabbing(); + + _transeiver->unbindAndClose(); + _isConnected = false; + } + } + + bool isConnected() + { + return _isConnected; + } + + sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + // send reset message + return (sl_result)_sendCommandWithoutResponse(SL_LIDAR_CMD_RESET); + } + + sl_result getAllSupportedScanModes(std::vector& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + Result ans = SL_RESULT_OK; + bool confProtocolSupported = false; + ans = checkSupportConfigCommands(confProtocolSupported, timeoutInMs); + if (!ans) return SL_RESULT_INVALID_DATA; + + if (confProtocolSupported) { + // 1. get scan mode count + sl_u16 modeCount; + ans = getScanModeCount(modeCount, timeoutInMs); + if (!ans) return ans; + // 2. for loop to get all fields of each scan mode + for (sl_u16 i = 0; i < modeCount; i++) { + LidarScanMode scanModeInfoTmp; + memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp)); + scanModeInfoTmp.id = i; + ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i, timeoutInMs); + if (!ans) return ans; + ans = getMaxDistance(scanModeInfoTmp.max_distance, i, timeoutInMs); + if (!ans) return ans; + ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i, timeoutInMs); + if (!ans) return ans; + ans = getScanModeName(scanModeInfoTmp.scan_mode, sizeof(scanModeInfoTmp.scan_mode), i, timeoutInMs); + if (!ans) return ans; + outModes.push_back(scanModeInfoTmp); + + } + return ans; + } + + return ans; + } + + sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + Result ans = SL_RESULT_OK; + std::vector answer; + bool lidarSupportConfigCmds = false; + ans = checkSupportConfigCommands(lidarSupportConfigCmds); + if (!ans) return ans; + + if (lidarSupportConfigCmds) { + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_TYPICAL, answer, nullptr, 0, timeoutInMs); + if (!ans) return ans; + if (answer.size() < sizeof(sl_u16)) { + return SL_RESULT_INVALID_DATA; + } + const sl_u16 *p_answer = reinterpret_cast(&answer[0]); + outMode = *p_answer; + return ans; + } + //old version of triangle lidar + else { + outMode = SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS; + return ans; + } + return ans; + + } + + sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + Result ans = SL_RESULT_OK; + bool ifSupportLidarConf = false; + LidarScanMode localMode; + + if (!isConnected()) return SL_RESULT_OPERATION_FAIL; + stop(); + + if (!outUsedScanMode) outUsedScanMode = &localMode; + + + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (!ans) return ans; + if (useTypicalScan){ + sl_u16 typicalMode; + ans = getTypicalScanMode(typicalMode); + if (!ans) return ans; + + //call startScanExpress to do the job + return startScanExpress(false, typicalMode, 0, outUsedScanMode); + } + + // 'useTypicalScan' is false, just use normal scan mode + + + return startScanNormal_commonpath(force, ifSupportLidarConf , *outUsedScanMode, DEFAULT_TIMEOUT); + } + + + // this path make sure the working mode has always been retrieved + sl_result startScanNormal_commonpath(bool force, bool ifSupportLidarConf, LidarScanMode& outUsedScanMode, sl_u32 timeout) + { + + Result ans = SL_RESULT_OK; + + if (ifSupportLidarConf) { + + outUsedScanMode.id = SL_LIDAR_CONF_SCAN_COMMAND_STD; + ans = getLidarSampleDuration(outUsedScanMode.us_per_sample, outUsedScanMode.id); + if (!ans) return ans; + ans = getMaxDistance(outUsedScanMode.max_distance, outUsedScanMode.id); + if (!ans) return ans; + ans = getScanModeAnsType(outUsedScanMode.ans_type, outUsedScanMode.id); + if (!ans) return ans; + ans = getScanModeName(outUsedScanMode.scan_mode, sizeof(outUsedScanMode.scan_mode), outUsedScanMode.id); + if (!ans) return ans; + + } + else { + // a legacy device + rplidar_response_sample_rate_t sampleRateTmp; + ans = _getLegacySampleDuration_uS(sampleRateTmp, timeout); + + if (!ans) return SL_RESULT_INVALID_DATA; + outUsedScanMode.us_per_sample = sampleRateTmp.std_sample_duration_us; + outUsedScanMode.max_distance = 16; + outUsedScanMode.ans_type = SL_LIDAR_ANS_TYPE_MEASUREMENT; + strcpy(outUsedScanMode.scan_mode, "Standard"); + } + + + _updateTimingDesc(_cached_DevInfo, outUsedScanMode.us_per_sample); + + startMotor(); + + _scanHolder.reset(); + _dataunpacker->enable(); + + ans = _sendCommandWithoutResponse(force ? SL_LIDAR_CMD_FORCE_SCAN : SL_LIDAR_CMD_SCAN, nullptr, 0, true); + if (ans) delay(10); // wait rplidar to handle it + return ans; + } + + + sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + LidarScanMode localMode; + bool ifSupportLidarConf; + + if (!isConnected()) return SL_RESULT_OPERATION_FAIL; + stop(); + + Result ans = checkSupportConfigCommands(ifSupportLidarConf); + if (!ans) return ans; + + return startScanNormal_commonpath(force, ifSupportLidarConf, localMode, timeout); + } + + sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + Result ans = SL_RESULT_OK; + if (!isConnected()) return SL_RESULT_OPERATION_FAIL; + stop(); //force the previous operation to stop + + LidarScanMode localMode; + + if (!outUsedScanMode) outUsedScanMode = &localMode; + + bool ifSupportLidarConf = false; + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (!ans) return SL_RESULT_INVALID_DATA; + + + + outUsedScanMode->id = scanMode; + if (ifSupportLidarConf) { + ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getScanModeName(outUsedScanMode->scan_mode, sizeof(outUsedScanMode->scan_mode), outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + } + else { + // legacy device support + if (scanMode != RPLIDAR_CONF_SCAN_COMMAND_STD) { + rplidar_response_sample_rate_t sampleRateTmp; + ans = _getLegacySampleDuration_uS(sampleRateTmp, timeout); + if (!ans) return RESULT_INVALID_DATA; + + outUsedScanMode->us_per_sample = sampleRateTmp.express_sample_duration_us; + outUsedScanMode->max_distance = 16; + outUsedScanMode->ans_type = SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; + strcpy(outUsedScanMode->scan_mode, "Express"); + } + else { + outUsedScanMode->ans_type = SL_LIDAR_ANS_TYPE_MEASUREMENT; + } + } + + if (outUsedScanMode->ans_type == SL_LIDAR_ANS_TYPE_MEASUREMENT) + { + // redirect to the correct function... + return startScanNormal(force, timeout); + } + + _updateTimingDesc(_cached_DevInfo, outUsedScanMode->us_per_sample); + startMotor(); + + _scanHolder.reset(); + _dataunpacker->enable(); + + sl_lidar_payload_express_scan_t scanReq; + memset(&scanReq, 0, sizeof(scanReq)); + + if (!ifSupportLidarConf) { + if (scanMode != SL_LIDAR_CONF_SCAN_COMMAND_STD && scanMode != SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS) + scanReq.working_mode = sl_u8(scanMode); + } + else + scanReq.working_mode = sl_u8(scanMode); + + scanReq.working_flags = options; + + ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq), true); + if (ans) delay(10); // wait rplidar to handle it + return ans; + + } + + sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + + + u_result ans = SL_RESULT_OK; + ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_STOP); + _disableDataGrabbing(); + + if (IS_FAIL(ans)) return ans; + + + delay(100); + + if(_isSupportingMotorCtrl == MotorCtrlSupportPwm) + setMotorSpeed(0); + + return SL_RESULT_OK; + } + + sl_result grabScanDataHqWithTimeStamp(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u64& timestamp_uS, sl_u32 timeout = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + + if (!nodebuffer) + return SL_RESULT_INVALID_DATA; + + auto availBuffer = _scanHolder.waitAndLockAvailableScan(timeout, ×tamp_uS); + if (!availBuffer) return SL_RESULT_OPERATION_TIMEOUT; + + count = std::min(count, availBuffer->size()); + + std::copy(availBuffer->begin(), availBuffer->begin() + count, nodebuffer); + + _scanHolder.unlockScan(availBuffer); + + return RESULT_OK; + } + + sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) + { + _u64 localTS; + return grabScanDataHqWithTimeStamp(nodebuffer, count, localTS, timeout); + } + + sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + u_result ans; + internal::message_autoptr_t ans_frame; + + ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_DEVICE_INFO, SL_LIDAR_ANS_TYPE_DEVINFO, ans_frame, timeout); + + if (IS_FAIL(ans)) return ans; + if (ans_frame->getPayloadSize() < sizeof(rplidar_response_device_info_t)) + { + return RESULT_INVALID_DATA; + } + info = *(rplidar_response_device_info_t*)ans_frame->getDataBuf(); +#ifdef _CPU_ENDIAN_BIG + info.firmware_version = le16_to_cpu(info.firmware_version); +#endif + + _cached_DevInfo = info; + return (sl_result)ans; + } + + sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + Result ans = SL_RESULT_OK; + support = MotorCtrlSupportNone; + _disableDataGrabbing(); + + { + sl_lidar_response_device_info_t devInfo; + ans = getDeviceInfo(devInfo, 500); + if (!ans) return ans; + sl_u8 majorId = devInfo.model >> 4; + if (majorId >= BUILTIN_MOTORCTL_MINUM_MAJOR_ID) { + support = MotorCtrlSupportRpm; + return ans; + } + else if(majorId >= A2A3_LIDAR_MINUM_MAJOR_ID){ + + rp::hal::AutoLocker l(_op_locker); + sl_lidar_payload_acc_board_flag_t flag; + flag.reserved = 0; + internal::message_autoptr_t ans_frame; + + ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_ACC_BOARD_FLAG, SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG, ans_frame, timeout, &flag, sizeof(flag)); + if (!ans) return ans; + + if (ans_frame->getPayloadSize() < sizeof(rplidar_response_acc_board_flag_t)) + { + return RESULT_INVALID_DATA; + } + + const sl_lidar_response_acc_board_flag_t* acc_board_flag + = reinterpret_cast(ans_frame->getDataBuf()); + + if (acc_board_flag->support_flag & SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) { + support = MotorCtrlSupportPwm; + } + return ans; + } + + } + return SL_RESULT_OK; + + } + + sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) + { + float sample_duration = scanMode.us_per_sample; + frequency = 1000000.0f / (count * sample_duration); + return SL_RESULT_OK; + } + + sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + sl_result ans = setLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, &conf, sizeof(sl_lidar_ip_conf_t), timeout); + return ans; + } + + sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + Result ans = SL_RESULT_OK; + std::vector reserve(2); //keep backward compatibility + + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, answer, &reserve[0], 2, timeout); + size_t len = answer.size(); + if (0 == len) return SL_RESULT_INVALID_DATA; + memcpy(&conf, &answer[0], len); + return ans; + } + + sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + u_result ans; + internal::message_autoptr_t ans_frame; + + ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_DEVICE_HEALTH, SL_LIDAR_ANS_TYPE_DEVHEALTH, ans_frame, timeout); + + if (IS_FAIL(ans)) return ans; + if (ans_frame->getPayloadSize() < sizeof(rplidar_response_device_health_t)) + { + return SL_RESULT_INVALID_DATA; + } + health = *(rplidar_response_device_health_t*)ans_frame->getDataBuf(); +#ifdef _CPU_ENDIAN_BIG + health.error_code = le16_to_cpu(health.error_code); +#endif + + return ans; + } + + sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + u_result ans; + + std::vector<_u8> answer(6, 0); + ans = getLidarConf(SL_LIDAR_CONF_LIDAR_MAC_ADDR, answer, NULL, 0, timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + size_t len = answer.size(); + if (0 == len) return SL_RESULT_INVALID_DATA; + memcpy(macAddrArray, &answer[0], len); + return ans; + } + + sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count) + { + return ascendScanData_(nodebuffer, count); + } + + sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count) + { + count = _rawSampleNodeHolder.waitAndFetch(nodebuffer, count, 0); + return SL_RESULT_OK; + } + + sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) + { + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + Result ans = SL_RESULT_OK; + + if(speed == DEFAULT_MOTOR_SPEED){ + sl_lidar_response_desired_rot_speed_t desired_speed; + ans = getDesiredSpeed(desired_speed); + if (ans) { + if (_isSupportingMotorCtrl == MotorCtrlSupportPwm) + speed = desired_speed.pwm_ref; + else + speed = desired_speed.rpm; + } + else { + //set a dummy default value + speed = 600; + } + } + switch (_isSupportingMotorCtrl) + { + case MotorCtrlSupportNone: + if (_transeiver->getBindedChannel()->getChannelType() == CHANNEL_TYPE_SERIALPORT) { + ISerialPortChannel* serialChanel = (ISerialPortChannel*)_transeiver->getBindedChannel(); + if (!speed) { + serialChanel->setDTR(true); + }else{ + serialChanel->setDTR(false); + } + } + break; + case MotorCtrlSupportPwm: + sl_lidar_payload_motor_pwm_t motor_pwm; + motor_pwm.pwm_value = speed; + + + ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_SET_MOTOR_PWM, &motor_pwm, sizeof(motor_pwm), true); + if (!ans) return ans; + delay(10); + break; + case MotorCtrlSupportRpm: + sl_lidar_payload_motor_pwm_t motor_rpm; + motor_rpm.pwm_value = speed; + + ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL, &motor_rpm, sizeof(motor_rpm), true); + if (!ans) return ans; + delay(10); + break; + } + return SL_RESULT_OK; + } + + sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs) + { + Result ans = SL_RESULT_OK; + rp::hal::AutoLocker l(_op_locker); + if (!isConnected()) return SL_RESULT_OPERATION_NOT_SUPPORT; + + + { + std::vector answer; + + ans = getLidarConf(SL_LIDAR_CONF_MIN_ROT_FREQ, answer); + if (!ans) return ans; + + const sl_u16 *min_answer = reinterpret_cast(&answer[0]); + motorInfo.min_speed = *min_answer; + + + ans = getLidarConf(SL_LIDAR_CONF_MAX_ROT_FREQ, answer); + if (!ans) return ans; + + const sl_u16 *max_answer = reinterpret_cast(&answer[0]); + motorInfo.max_speed = *max_answer; + + sl_lidar_response_desired_rot_speed_t desired_speed; + ans = getDesiredSpeed(desired_speed); + if (!ans) return ans; + if(motorInfo.motorCtrlSupport == MotorCtrlSupportPwm) + motorInfo.desired_speed = desired_speed.pwm_ref; + else + motorInfo.desired_speed = desired_speed.rpm; + + } + return SL_RESULT_OK; + } + + sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected) + { + // ask the LIDAR to stop working first... + stop(); + + rp::hal::AutoLocker l(_op_locker); + + IChannel* cachedChannel = _transeiver->getBindedChannel(); + if (!cachedChannel) return SL_RESULT_OPERATION_FAIL; + if (cachedChannel->getChannelType() != CHANNEL_TYPE_SERIALPORT) + { + // only works for UART connection + return RESULT_OPERATION_NOT_SUPPORT; + } + + // disable the transeiver as it may interrupt the operation... + _transeiver->unbindAndClose(); + + sl_result ans = SL_RESULT_OK; + + do { + // reopen the channel... + + if (!cachedChannel->open()) { + // failed to reopen + // try to revert back... + ans = SL_RESULT_OPERATION_FAIL; + break; + } + + cachedChannel->flush(); + + // wait for a while + delay(10); + cachedChannel->clearReadCache(); + + // sending magic byte to let the target LIDAR start baudrate measurement + // More than 100 bytes per second datarate is required to trigger the measurements + { + + + sl_u8 magicByteSeq[16]; + + memset(magicByteSeq, SL_LIDAR_AUTOBAUD_MAGICBYTE, sizeof(magicByteSeq)); + + sl_u64 startTS = getms(); + + while (getms() - startTS < 1500) //lasting for 1.5sec + { + if (cachedChannel->write(magicByteSeq, sizeof(magicByteSeq)) < 0) + { + ans = SL_RESULT_OPERATION_FAIL; + break; + } + + size_t dataCountGot; + if (cachedChannel->waitForData(1, 1, &dataCountGot)) { + //got reply, stop + ans = SL_RESULT_OK; + break; + } + } + } + + if (IS_FAIL(ans)) break; + + // getback the bps measured + _u32 bpsDetected = 0; + size_t dataCountGot; + if (cachedChannel->waitForData(4, 500, &dataCountGot)) { + //got reply, stop + cachedChannel->read(&bpsDetected, 4); + if (baudRateDetected) *baudRateDetected = bpsDetected; + + + cachedChannel->close(); + // restart the transiever + ans = _transeiver->openChannelAndBind(cachedChannel); + if (IS_FAIL(ans)) return ans; + + + // send a confirmation to the LIDAR, otherwise, the previous baudrate will be reverted back + sl_lidar_payload_new_bps_confirmation_t confirmation; + confirmation.flag = 0x5F5F; + confirmation.required_bps = requiredBaudRate; + confirmation.param = 0; + + + ans = _sendCommandWithoutResponse(SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM, &confirmation, sizeof(confirmation)); + + return ans; + } + } while (0); + + _transeiver->openChannelAndBind(cachedChannel); + + return ans; + } + + protected: + sl_result startMotor() + { + return setMotorSpeed(DEFAULT_MOTOR_SPEED); + } + + u_result getDesiredSpeed(sl_lidar_response_desired_rot_speed_t & motorSpeed, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + u_result ans; + std::vector answer; + ans = getLidarConf(SL_LIDAR_CONF_DESIRED_ROT_FREQ, answer, nullptr, 0, timeoutInMs); + + if (IS_FAIL(ans)) return ans; + + const sl_lidar_response_desired_rot_speed_t *p_answer = reinterpret_cast(&answer[0]); + motorSpeed = *p_answer; + return RESULT_OK; + } + + u_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + u_result ans; + rplidar_response_device_info_t devinfo; + ans = getDeviceInfo(devinfo, timeoutInMs); + if (IS_FAIL(ans)) { + outSupport = false; + return ans; + } + + + if (_checkNDMagicNumber(devinfo.model)) { + + outSupport = true; + } + else { + // if lidar firmware >= 1.24 + outSupport = (devinfo.firmware_version >= ((0x1 << 8) | 24)); + } + return RESULT_OK; + } + + + u_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + u_result ans; + std::vector<_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_COUNT, answer); + if (IS_FAIL(ans)) { + return ans; + } + if (answer.size() < sizeof(_u16)) { + return RESULT_INVALID_DATA; + } + const _u16* p_answer = reinterpret_cast(&answer[0]); + modeCount = *p_answer; + return ans; + } + + u_result setLidarConf(_u32 type, const void* payload, size_t payloadSize, _u32 timeout) + { + if (type < 0x00010000 || type >0x0001FFFF) + return SL_RESULT_INVALID_DATA; + + + std::vector requestPkt; + requestPkt.resize(sizeof(sl_lidar_payload_set_scan_conf_t) + payloadSize); + if (!payload) payloadSize = 0; + sl_lidar_payload_set_scan_conf_t* query = reinterpret_cast(&requestPkt[0]); + + query->type = type; + + if (payloadSize) + memcpy(&query[1], payload, payloadSize); + + sl_result ans; + internal::message_autoptr_t ans_frame; + ans = _sendCommandWithResponse(SL_LIDAR_CMD_SET_LIDAR_CONF, SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF, ans_frame, timeout, &requestPkt[0], requestPkt.size()); + + if (IS_FAIL(ans)) { + return ans; + } + + //check if returned size is even less than sizeof(type) + if (ans_frame->getPayloadSize() < sizeof(rplidar_response_set_lidar_conf_t)) { + return RESULT_INVALID_DATA; + } + + const rplidar_response_set_lidar_conf_t* response = + reinterpret_cast(ans_frame->getDataBuf()); + + + if (ans_frame->getPayloadSize() == 4) { + // legacy device? + return *(const u_result*)(ans_frame->getDataBuf()); + } + else { + if (response->type != type) { + return RESULT_INVALID_DATA; + } + + return (u_result)response->result; + } + } + + u_result getLidarConf(_u32 type, std::vector<_u8>& outputBuf, const void* payload = NULL, size_t payloadSize = 0, _u32 timeout = DEFAULT_TIMEOUT) + { + std::vector<_u8> requestPkt; + + if (!payload) payloadSize = 0; + requestPkt.resize(sizeof(rplidar_payload_get_scan_conf_t) + payloadSize); + rplidar_payload_get_scan_conf_t* query = reinterpret_cast(&requestPkt[0]); + + query->type = type; + + if (payloadSize) + memcpy(&query[1], payload, payloadSize); + + u_result ans; + internal::message_autoptr_t ans_frame; + ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_LIDAR_CONF, SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF, ans_frame, timeout, &requestPkt[0], requestPkt.size()); + if (IS_FAIL(ans)) { + return ans; + } + //check if returned size is even less than sizeof(type) + if (ans_frame->getPayloadSize() < offsetof(rplidar_response_get_lidar_conf_t, payload)) { + return SL_RESULT_INVALID_DATA; + } + + //check if returned type is same as asked type + const rplidar_response_get_lidar_conf_t* replied = + reinterpret_cast(ans_frame->getDataBuf()); + + + if (replied->type != type) { + return SL_RESULT_INVALID_DATA; + } + //copy all the payload into &outputBuf + int payLoadLen = (int)ans_frame->getPayloadSize() - (int)offsetof(rplidar_response_get_lidar_conf_t, payload); + //do consistency check + if (payLoadLen < 0) { + return SL_RESULT_INVALID_DATA; + } + //copy all payLoadLen bytes to outputBuf + outputBuf.resize(payLoadLen); + if (payLoadLen) + memcpy(&outputBuf[0], replied->payload, payLoadLen); + return ans; + } + + u_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + u_result ans; + + std::vector<_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, &scanModeID, sizeof(_u16), timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + if (answer.size() < sizeof(_u32)) + { + return SL_RESULT_INVALID_DATA; + } + const _u32* result = reinterpret_cast(&answer[0]); + sampleDurationRes = (float)(*result / 256.0); + return ans; + } + + u_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + u_result ans; + + + std::vector<_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, &scanModeID, sizeof(_u16), timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + if (answer.size() < sizeof(_u32)) + { + return SL_RESULT_INVALID_DATA; + } + const _u32* result = reinterpret_cast(&answer[0]); + maxDistance = (float)(*result >> 8); + return ans; + } + + u_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + u_result ans; + + std::vector<_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, &scanModeID, sizeof(_u16), timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + if (answer.size() < sizeof(_u8)) + { + return SL_RESULT_INVALID_DATA; + } + const _u8* result = reinterpret_cast(&answer[0]); + ansType = *result; + return ans; + } + + u_result getScanModeName(char* modeName, size_t stringSize, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) + { + u_result ans; + + std::vector<_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_NAME, answer, &scanModeID, sizeof(_u16), timeoutInMs); + if (IS_FAIL(ans)) + { + return ans; + } + size_t len = std::min(answer.size(), stringSize); + if (0 == len) return SL_RESULT_INVALID_DATA; + + memcpy(modeName, &answer[0], len); + return ans; + } + + + static LIDARTechnologyType ParseLIDARTechnologyTypeByModelID(_u8 modelID) + { + _u8 majorModelID = (modelID >> 4); + // FIXME: stupid implementation here + if (majorModelID < NEWDESIGN_MINUM_MAJOR_ID) { + return LIDAR_TECHNOLOGY_TRIANGULATION; + } + else { + return LIDAR_TECHNOLOGY_DTOF; + } + } + + static LIDARMajorType ParseLIDARMajorTypeByModelID(_u8 modelID) + { + _u8 majorModelID = (modelID >> 4); + + + if (majorModelID >= TOF_M_SERIAL_MINUM_MAJOR_ID) { + return LIDAR_MAJOR_TYPE_M_SERIES; + } + else if (majorModelID >= TOF_T_SERIAL_MINUM_MAJOR_ID) { + return LIDAR_MAJOR_TYPE_T_SERIES; + } + else if (majorModelID >= TOF_S_SERIAL_MINUM_MAJOR_ID) { + return LIDAR_MAJOR_TYPE_S_SERIES; + } + else if (majorModelID >= TOF_C_SERIAL_MINUM_MAJOR_ID) { + return LIDAR_MAJOR_TYPE_C_SERIES; + } + else { + return LIDAR_MAJOR_TYPE_A_SERIES; + } + } + + static std::string GetModelNameStringByModelID(_u8 modelID) + { + + char stringBuffer[100]; + auto majorType = ParseLIDARMajorTypeByModelID(modelID); + + + switch (majorType) { + case LIDAR_MAJOR_TYPE_A_SERIES: + sprintf(stringBuffer, "A%dM%d", (modelID >> 4), (modelID & 0xF)); + + break; + + case LIDAR_MAJOR_TYPE_S_SERIES: + sprintf(stringBuffer, "S%dM%d", (modelID >> 4) - (TOF_S_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF)); + + break; + + case LIDAR_MAJOR_TYPE_T_SERIES: + sprintf(stringBuffer, "T%dM%d", (modelID >> 4) - (TOF_T_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF)); + + break; + + case LIDAR_MAJOR_TYPE_M_SERIES: + sprintf(stringBuffer, "M%dM%d", (modelID >> 4) - (TOF_M_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF)); + + break; + + case LIDAR_MAJOR_TYPE_C_SERIES: + sprintf(stringBuffer, "C%dM%d", (modelID >> 4) - (TOF_C_SERIAL_MINUM_MAJOR_ID)+1, (modelID & 0xF)); + + break; + + + default: + sprintf(stringBuffer, "unknown(%x)", modelID); + } + + return std::string(stringBuffer); + } + + protected: + + void _disableDataGrabbing() + { + _dataunpacker->disable(); + _protocolHandler->exitLoopMode(); // exit loop mode + } + + + + bool _checkNDMagicNumber(_u8 model) + { + return ((model >> 4) >= NEWDESIGN_MINUM_MAJOR_ID); + } + + + + + u_result _detectLIDARNativeInterfaceType(LIDARInterfaceType & outputType, const rplidar_response_device_info_t& devInfo, sl_u32 timeout = DEFAULT_TIMEOUT) + { + + LIDARMajorType majorType = ParseLIDARMajorTypeByModelID(devInfo.model); + + switch (majorType) { + case LIDAR_MAJOR_TYPE_A_SERIES: + case LIDAR_MAJOR_TYPE_M_SERIES: + case LIDAR_MAJOR_TYPE_C_SERIES: + + outputType = LIDAR_INTERFACE_UART; + return SL_RESULT_OK; + + + case LIDAR_MAJOR_TYPE_T_SERIES: + outputType = LIDAR_INTERFACE_ETHERNET; + return SL_RESULT_OK; + + case LIDAR_MAJOR_TYPE_S_SERIES: + { + // ethernet version exists, check whether it is + _u8 macAddr[6]; + u_result ans = getDeviceMacAddr(macAddr, timeout); + if (IS_FAIL(ans)) { + // cannot retrieve the device mac address, consider a UART interface version + outputType = LIDAR_INTERFACE_UART; + } + else { + outputType = LIDAR_INTERFACE_ETHERNET; + } + return SL_RESULT_OK; + } + + + case LIDAR_MAJOR_TYPE_UNKNOWN: + default: + outputType = LIDAR_INTERFACE_UNKNOWN; + return SL_RESULT_OK; + } + } + + _u32 _getNativeBaudRate(const rplidar_response_device_info_t & devInfo) + { + _u8 majorModelID = (devInfo.model >> 4); + switch (majorModelID) + { + case 1: + case 2: + case 3: //A1..A3 series + return (devInfo.hardware_version >= 6) ? 256000 : 115200; + case 4: //C series + return 460800; + case 6: //model ID of S1 + return 256000; + case 7: //model ID of S2 + case 8: //model ID of S3 + if (devInfo.model == (0x82)) return 460800; + return 1000000; + default: + return 0; //0 as unknown + } + } + + bool _updateTimingDesc(const rplidar_response_device_info_t& devInfo, float selectedSampleDuration) + { + _timing_desc.native_baudrate = _getNativeBaudRate(devInfo); + _detectLIDARNativeInterfaceType(_timing_desc.native_interface_type, devInfo, 500); + + _timing_desc.sample_duration_uS = (_u64)(selectedSampleDuration + 0.5f); + + //FIXME: will be changed in future releases + _timing_desc.native_timestamp_support = false; + _timing_desc.linkage_delay_uS = 0; + + + // notify the data unpacker + _dataunpacker->updateUnpackerContext(internal::LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING ,&_timing_desc, sizeof(_timing_desc)); + return true; + + } + + u_result _getLegacySampleDuration_uS(rplidar_response_sample_rate_t& rateInfo, _u32 timeout) + { + + static const _u32 LEGACY_SAMPLE_DURATION = 476; + + rplidar_response_device_info_t devinfo; + // 1. fetch the device version first... + u_result ans = getDeviceInfo(devinfo, timeout); + + rateInfo.express_sample_duration_us = LEGACY_SAMPLE_DURATION; + rateInfo.std_sample_duration_us = LEGACY_SAMPLE_DURATION; + + if (IS_FAIL(ans)) { + return ans; + } + + if (getLIDARMajorType(&devinfo) == LIDAR_MAJOR_TYPE_A_SERIES) { + if (devinfo.firmware_version < ((0x1 << 8) | 17)) { + // very very rare and old model found!! + return SL_RESULT_OK; + } + } + + + internal::message_autoptr_t ans_frame; + + ans = _sendCommandWithResponse(SL_LIDAR_CMD_GET_SAMPLERATE, SL_LIDAR_ANS_TYPE_SAMPLE_RATE, ans_frame, timeout); + + if (IS_FAIL(ans)) return ans; + if (ans_frame->getPayloadSize() < sizeof(rplidar_response_sample_rate_t)) + { + return RESULT_INVALID_DATA; + } + memcpy(&rateInfo, ans_frame->getDataBuf(), sizeof(rateInfo)); + +#ifdef _CPU_ENDIAN_BIG + rateInfo.express_sample_duration_us = le16_to_cpu(rateInfo.express_sample_duration_us); + rateInfo.std_sample_duration_us = le16_to_cpu(rateInfo.std_sample_duration_us); +#endif + + return ans; + } + + + u_result _sendCommandWithoutResponse(_u8 cmd, const void* payload = NULL, size_t payloadsize = 0, bool noForceStop = false) + { + if (!noForceStop) { + _disableDataGrabbing(); + } + _response_waiter.set(false); + + internal::message_autoptr_t message(new internal::ProtocolMessage(cmd, (const _u8*)payload, payloadsize)); + return _transeiver->sendMessage(message); + + } + + u_result _sendCommandWithResponse(_u8 cmd, _u8 responseType, internal::message_autoptr_t& ansPkt, _u32 timeout = DEFAULT_TIMEOUT, const void* payload = NULL, size_t payloadsize = 0) + { + u_result ans; + + _data_locker.lock(); + + internal::message_autoptr_t message(new internal::ProtocolMessage(cmd, (const _u8*)payload, payloadsize)); + _disableDataGrabbing(); + _waiting_packet_type = responseType; + _response_waiter.set(false); + _data_locker.unlock(); + + ans = _transeiver->sendMessage(message); + + if (IS_FAIL(ans)) return ans; + + do { + switch (_response_waiter.wait(timeout)) { + case rp::hal::Event::EVENT_TIMEOUT: + return RESULT_OPERATION_TIMEOUT; + case rp::hal::Event::EVENT_OK: + _data_locker.lock(); + ansPkt = _lastAnsPkt; + _data_locker.unlock(); + return RESULT_OK; + default: + return RESULT_OPERATION_FAIL; + } + } while (1); + } + + public: + + virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) + { + _scanHolder.pushScanNodeData(timestamp_uS, node); + _rawSampleNodeHolder.pushNode(timestamp_uS, node); + } + + virtual void onHQNodeScanResetReq() { + _scanHolder.rewindCurrentScanData(); + } + + virtual void onProtocolMessageDecoded(const internal::ProtocolMessage& msg) + { + internal::message_autoptr_t message = std::make_shared(msg); + + if (_dataunpacker->onSampleData(message->cmd, message->getDataBuf(), message->getPayloadSize())) + { + return; + } + + if (message->cmd == _waiting_packet_type) { + _data_locker.lock(); + _lastAnsPkt = message; + _response_waiter.setResult(message->cmd); + _data_locker.unlock(); + } + + + } + private: + + std::shared_ptr _protocolHandler; + std::shared_ptr _transeiver; + std::shared_ptr _dataunpacker; + + bool _isConnected; + + MotorCtrlSupport _isSupportingMotorCtrl; + + + rp::hal::Locker _op_locker; + rp::hal::Locker _data_locker; + rp::hal::Waiter<_u32> _response_waiter; + + ScanDataHolder _scanHolder; + RawSampleNodeHolder _rawSampleNodeHolder; + _u32 _waiting_packet_type; + internal::message_autoptr_t _lastAnsPkt; + + sl_lidar_response_device_info_t _cached_DevInfo; + SlamtecLidarTimingDesc _timing_desc; + + }; + + Result createLidarDriver() + { + return new SlamtecLidarDriver(); + } +} \ No newline at end of file diff --git a/sdk/src/sl_lidarprotocol_codec.cpp b/sdk/src/sl_lidarprotocol_codec.cpp new file mode 100644 index 0000000..a629000 --- /dev/null +++ b/sdk/src/sl_lidarprotocol_codec.cpp @@ -0,0 +1,238 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + + + +#include "sdkcommon.h" +#include "hal/byteorder.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" + +#include "sl_lidar_driver.h" +#include "sl_crc.h" +#include + +#include "sl_async_transceiver.h" +#include "sl_lidarprotocol_codec.h" + + + +namespace sl { namespace internal { + + + +RPLidarProtocolCodec::RPLidarProtocolCodec() + : IAsyncProtocolCodec() + , _listener(NULL) + , _op_locker(true) +{ + onDecodeReset(); +} + +void RPLidarProtocolCodec::exitLoopMode() { + onDecodeReset(); +} + + + +void RPLidarProtocolCodec::setMessageListener(IProtocolMessageListener* listener) +{ + rp::hal::AutoLocker l(_op_locker); + _listener = listener; +} + +size_t RPLidarProtocolCodec::estimateLength(message_autoptr_t& message) +{ + size_t actualSize = 2; //1-byte's sync byte, 1-byte's cmd byte + + if (message->cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) { + actualSize += (message->getPayloadSize() & 0xFF); + actualSize += 2; //1-byte for size field, 1-byte for checksum + } + + return actualSize; +} + + +void RPLidarProtocolCodec::onEncodeData(message_autoptr_t& message, _u8* buffer, size_t* size) +{ + _u8 checksum = 0; + size_t writeSize = std::min(*size, estimateLength(message)); + size_t currentPos = 0; + + while (currentPos < writeSize) { + _u8 currentTxByte; + switch (currentPos) { + case 0: // sync byte + currentTxByte = RPLIDAR_CMD_SYNC_BYTE; + break; + case 1: // cmd byte + currentTxByte = message->cmd; + break; + case 2: // size byte + currentTxByte = (_u8)message->getPayloadSize(); + break; + default: + { + size_t payloadPos = currentPos - 3; + if (payloadPos == message->getPayloadSize()) { + // checksum byte + currentTxByte = checksum; + assert(currentPos + 1 == writeSize); + } + else { + // payload + currentTxByte = message->getDataBuf()[payloadPos]; + } + } + } + + + checksum ^= currentTxByte; + buffer[currentPos++] = currentTxByte; + } while (0); + + *size = currentPos; +} + +void RPLidarProtocolCodec::onDecodeReset() { + rp::hal::AutoLocker autolock(_op_locker); + // flush the pending data + _decodingMessage.cleanData(); + // reset to initial state + _rx_pos = 0; + _working_states = STATUS_WAIT_SYNC1; +} + + +void RPLidarProtocolCodec::onDecodeData(const void* buffer, size_t size) +{ + rp::hal::AutoLocker autolock(_op_locker); + + const _u8* data = reinterpret_cast(buffer); + const _u8* dataEnd = data + size; + + + while (data != dataEnd) { + _u8 currentByte = *data; + ++data; + + switch (_working_states & ((_u32)STATUS_LOOP_MODE_FLAG - 1)) { + case STATUS_WAIT_SYNC1: + if (currentByte == RPLIDAR_ANS_SYNC_BYTE1) { + _working_states = STATUS_WAIT_SYNC2; + } + break; + case STATUS_WAIT_SYNC2: + if (currentByte == RPLIDAR_ANS_SYNC_BYTE2) { + _working_states = STATUS_WAIT_SIZE_FLAG; + _rx_pos = 0; // init rx pos for recv size and flag + } + else { + // reset to the initial state + _working_states = STATUS_WAIT_SYNC1; + } + break; + case STATUS_WAIT_SIZE_FLAG: + { + assert(sizeof(_decodingMessage.len) >= 4); + _u8* byteArr = reinterpret_cast<_u8*>(&_decodingMessage.len); + byteArr[_rx_pos++] = currentByte; + + if (_rx_pos == 4) { + _working_states = STATUS_WAIT_TYPE; + _decodingMessage.len = le32_to_cpu(_decodingMessage.len); + + // 30bit size + 2bit flag has been received + _u32 flagbits = (_u32)(_decodingMessage.len >> RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT); + if (flagbits & RPLIDAR_ANS_PKTFLAG_LOOP) { + _working_states |= STATUS_LOOP_MODE_FLAG; + } + _decodingMessage.len = (_decodingMessage.len & RPLIDAR_ANS_HEADER_SIZE_MASK); + // alloc buffer + _decodingMessage.fillData(NULL, _decodingMessage.getPayloadSize()); + _rx_pos = 0; + } + } + break; + case STATUS_WAIT_TYPE: + // save the type field as a cmd + _decodingMessage.cmd = currentByte; + + // recv payload... + _working_states = (_working_states & STATUS_LOOP_MODE_FLAG) + | STATUS_RECV_PAYLOAD; + + if (!_decodingMessage.getPayloadSize()) { + // zero payload packet? + _working_states = STATUS_WAIT_SYNC1; + } + break; + case STATUS_RECV_PAYLOAD: + _decodingMessage.getDataBuf()[_rx_pos++] = currentByte; + + if ((size_t)_rx_pos == _decodingMessage.getPayloadSize()) { + if (_working_states & STATUS_LOOP_MODE_FLAG) { + // rewind to the payload recv status in loop mode + _rx_pos = 0; + } + else { + // reset the decoder + _working_states = STATUS_WAIT_SYNC1; + } + + IProtocolMessageListener* cachedLister = _listener; + + autolock.forceUnlock(); //unlock the oplock to prevent deadlock + + + if (cachedLister) { + cachedLister->onProtocolMessageDecoded(_decodingMessage); + } + + _op_locker.lock(); // relock it + } + break; + } + + } +} + + + + +}} \ No newline at end of file diff --git a/sdk/src/sl_lidarprotocol_codec.h b/sdk/src/sl_lidarprotocol_codec.h new file mode 100644 index 0000000..1cc62f7 --- /dev/null +++ b/sdk/src/sl_lidarprotocol_codec.h @@ -0,0 +1,88 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + + +#pragma once + +#include "sl_async_transceiver.h" + +namespace sl { namespace internal { + + +class IProtocolMessageListener { +public: + virtual void onProtocolMessageDecoded(const ProtocolMessage&) = 0; +}; + + +class RPLidarProtocolCodec : public IAsyncProtocolCodec +{ +public: + + enum { + STATUS_WAIT_SYNC1 = 0x0, + STATUS_WAIT_SYNC2 = 0x1, + STATUS_WAIT_SIZE_FLAG = 0x2, + STATUS_WAIT_TYPE = 0x3, + STATUS_RECV_PAYLOAD = 0x4, + STATUS_LOOP_MODE_FLAG = 0x80000000, + }; + + RPLidarProtocolCodec(); + + void exitLoopMode(); + + + virtual size_t estimateLength(message_autoptr_t& message); + + + virtual void onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size); + + virtual void onDecodeReset(); + virtual void onDecodeData(const void* buffer, size_t size); + + void setMessageListener(IProtocolMessageListener* l); + +protected: + + IProtocolMessageListener* _listener; + ProtocolMessage _decodingMessage; + rp::hal::Locker _op_locker; + + _u32 _working_states; + int _rx_pos; +}; + +}} + + + diff --git a/sdk/src/sl_serial_channel.cpp b/sdk/src/sl_serial_channel.cpp new file mode 100644 index 0000000..f7faf3c --- /dev/null +++ b/sdk/src/sl_serial_channel.cpp @@ -0,0 +1,146 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + + class SerialPortChannel : public ISerialPortChannel + { + public: + SerialPortChannel(const std::string& device, int baudrate) :_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()) + { + _device = device; + _baudrate = baudrate; + } + + ~SerialPortChannel() + { + if (_rxtxSerial) + delete _rxtxSerial; + } + + bool bind(const std::string& device, sl_s32 baudrate) + { + _closePending = false; + return _rxtxSerial->bind(device.c_str(), baudrate); + } + + bool open() + { + if(!bind(_device, _baudrate)) + return false; + return _rxtxSerial->open(); + } + + void close() + { + _closePending = true; + _rxtxSerial->cancelOperation(); + _rxtxSerial->close(); + } + void flush() + { + _rxtxSerial->flush(0); + } + + sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs) + { + _word_size_t result; + size_t size_holder; + size_hint = 0; + + if (_closePending) return RESULT_OPERATION_TIMEOUT; + + if (!_rxtxSerial->isOpened()) { + return RESULT_OPERATION_FAIL; + } + + result = _rxtxSerial->waitfordata(1, timeoutInMs, &size_holder); + size_hint = size_holder; + if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_DEV_ERR) + return RESULT_OPERATION_FAIL; + if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_TIMEOUT) + return RESULT_OPERATION_TIMEOUT; + + return RESULT_OK; + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (_closePending) return false; + return (_rxtxSerial->waitfordata(size, timeoutInMs, actualReady) == rp::hal::serial_rxtx::ANS_OK); + } + + int write(const void* data, size_t size) + { + return _rxtxSerial->senddata((const sl_u8 * )data, size); + } + + int read(void* buffer, size_t size) + { + size_t lenRec = 0; + lenRec = _rxtxSerial->recvdata((sl_u8 *)buffer, size); + return (int)lenRec; + } + + void clearReadCache() + { + + } + + void setDTR(bool dtr) + { + dtr ? _rxtxSerial->setDTR() : _rxtxSerial->clearDTR(); + } + + int getChannelType() { + return CHANNEL_TYPE_SERIALPORT; + } + + private: + rp::hal::serial_rxtx * _rxtxSerial; + bool _closePending; + std::string _device; + int _baudrate; + + }; + + Result createSerialPortChannel(const std::string& device, int baudrate) + { + return new SerialPortChannel(device, baudrate); + } + +} \ No newline at end of file diff --git a/sdk/src/sl_tcp_channel.cpp b/sdk/src/sl_tcp_channel.cpp new file mode 100644 index 0000000..8351c41 --- /dev/null +++ b/sdk/src/sl_tcp_channel.cpp @@ -0,0 +1,124 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + + class TcpChannel : public IChannel + { + public: + TcpChannel(const std::string& ip, int port) : _binded_socket(rp::net::StreamSocket::CreateSocket()) { + _ip = ip; + _port = port; + } + + bool bind(const std::string & ip, sl_s32 port) + { + _socket = rp::net::SocketAddress(ip.c_str(), port); + return true; + } + + bool open() + { + if(!bind(_ip, _port)) + return false; + return IS_OK(_binded_socket->connect(_socket)); + + } + + void close() + { + _binded_socket->dispose(); + _binded_socket = NULL; + } + void flush() + { + + } + + sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs) + { + u_result ans; + size_hint = 0; + ans = _binded_socket->waitforData(timeoutInMs); + + switch (ans) { + case RESULT_OK: + size_hint = 1024; //dummy value + break; + } + + return ans; + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (actualReady) + *actualReady = size; + return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK); + + } + + int write(const void* data, size_t size) + { + return _binded_socket->send(data, size); + } + + int read(void* buffer, size_t size) + { + size_t lenRec = 0; + _binded_socket->recv(buffer, size, lenRec); + return (int)lenRec; + } + + void clearReadCache() {} + + void setStatus(_u32 flag){} + + int getChannelType() { + return CHANNEL_TYPE_TCP; + } + private: + rp::net::StreamSocket * _binded_socket; + rp::net::SocketAddress _socket; + std::string _ip; + int _port; + }; + Result createTcpChannel(const std::string& ip, int port) + { + return new TcpChannel(ip, port); + } +} \ No newline at end of file diff --git a/sdk/src/sl_udp_channel.cpp b/sdk/src/sl_udp_channel.cpp new file mode 100644 index 0000000..df0808d --- /dev/null +++ b/sdk/src/sl_udp_channel.cpp @@ -0,0 +1,129 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + class UdpChannel : public IChannel + { + public: + UdpChannel(const std::string& ip, int port) : _binded_socket(rp::net::DGramSocket::CreateSocket()) { + _ip = ip; + _port = port; + } + + bool bind(const std::string & ip, sl_s32 port) + { + _socket = rp::net::SocketAddress(ip.c_str(), port); + return true; + } + + bool open() + { + if(!bind(_ip, _port)) + return false; + return SL_IS_OK(_binded_socket->setPairAddress(&_socket)); + } + + void close() + { + _binded_socket->dispose(); + _binded_socket = NULL; + } + void flush() + { + clearReadCache(); + } + + sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs) + { + u_result ans; + size_hint = 0; + ans = _binded_socket->waitforData(timeoutInMs); + + switch (ans) { + case RESULT_OK: + size_hint = 1024; //dummy value + break; + } + + return ans; + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (actualReady) + *actualReady = size; + return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK); + + } + + int write(const void* data, size_t size) + { + return _binded_socket->sendTo(nullptr, data, size); + } + + int read(void* buffer, size_t size) + { + size_t actualGet; + + u_result ans = _binded_socket->recvFrom(buffer, size, actualGet); + if (IS_FAIL(ans)) return 0; + return actualGet; + + } + + void clearReadCache() { + _binded_socket->clearRxCache(); + } + + void setStatus(_u32 flag){} + + int getChannelType() { + return CHANNEL_TYPE_UDP; + } + + private: + rp::net::DGramSocket * _binded_socket; + rp::net::SocketAddress _socket; + std::string _ip; + int _port; + }; + + Result createUdpChannel(const std::string& ip, int port) + { + return new UdpChannel(ip, port); + } +} \ No newline at end of file diff --git a/src/sllidar_client.cpp b/src/sllidar_client.cpp new file mode 100644 index 0000000..6441939 --- /dev/null +++ b/src/sllidar_client.cpp @@ -0,0 +1,43 @@ +/* + * SLLIDAR ROS2 CLIENT + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2022 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include + +#define RAD2DEG(x) ((x)*180./M_PI) + +static void scanCb(sensor_msgs::msg::LaserScan::SharedPtr scan) { + int count = scan->scan_time / scan->time_increment; + printf("[SLLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count); + printf("[SLLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min), + RAD2DEG(scan->angle_max)); + + for (int i = 0; i < count; i++) { + float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); + printf("[SLLIDAR INFO]: angle-distance : [%f, %f]\n", degree, scan->ranges[i]); + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("sllidar_client"); + + auto lidar_info_sub = node->create_subscription( + "scan", rclcpp::SensorDataQoS(), scanCb); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + + return 0; +} diff --git a/src/sllidar_node.cpp b/src/sllidar_node.cpp new file mode 100644 index 0000000..5abfd0f --- /dev/null +++ b/src/sllidar_node.cpp @@ -0,0 +1,483 @@ +/* + * SLLIDAR ROS2 NODE + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2022 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include +#include "sl_lidar.h" +#include "math.h" + +#include + +#ifndef _countof +#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) +#endif + +#define DEG2RAD(x) ((x)*M_PI/180.) + +#define ROS2VERSION "1.0.1" + +using namespace sl; + +bool need_exit = false; + +class SLlidarNode : public rclcpp::Node +{ + public: + SLlidarNode() + : Node("sllidar_node") + { + + scan_pub = this->create_publisher("scan", rclcpp::QoS(rclcpp::KeepLast(10))); + + } + + private: + void init_param() + { + this->declare_parameter("channel_type","serial"); + this->declare_parameter("tcp_ip", "192.168.0.7"); + this->declare_parameter("tcp_port", 20108); + this->declare_parameter("udp_ip","192.168.11.2"); + this->declare_parameter("udp_port",8089); + this->declare_parameter("serial_port", "/dev/ttyUSB0"); + this->declare_parameter("serial_baudrate",1000000); + this->declare_parameter("frame_id","laser_frame"); + this->declare_parameter("inverted", false); + this->declare_parameter("angle_compensate", false); + this->declare_parameter("scan_mode",std::string()); + this->declare_parameter("scan_frequency",10); + + this->get_parameter_or("channel_type", channel_type, "serial"); + this->get_parameter_or("tcp_ip", tcp_ip, "192.168.0.7"); + this->get_parameter_or("tcp_port", tcp_port, 20108); + this->get_parameter_or("udp_ip", udp_ip, "192.168.11.2"); + this->get_parameter_or("udp_port", udp_port, 8089); + this->get_parameter_or("serial_port", serial_port, "/dev/ttyUSB0"); + this->get_parameter_or("serial_baudrate", serial_baudrate, 1000000/*256000*/);//ros run for A1 A2, change to 256000 if A3 + this->get_parameter_or("frame_id", frame_id, "laser_frame"); + this->get_parameter_or("inverted", inverted, false); + this->get_parameter_or("angle_compensate", angle_compensate, false); + this->get_parameter_or("scan_mode", scan_mode, std::string()); + if(channel_type == "udp") + this->get_parameter_or("scan_frequency", scan_frequency, 20.0); + else + this->get_parameter_or("scan_frequency", scan_frequency, 10.0); + } + + bool getSLLIDARDeviceInfo(ILidarDriver * drv) + { + sl_result op_result; + sl_lidar_response_device_info_t devinfo; + + op_result = drv->getDeviceInfo(devinfo); + if (SL_IS_FAIL(op_result)) { + if (op_result == SL_RESULT_OPERATION_TIMEOUT) { + RCLCPP_ERROR(this->get_logger(),"Error, operation time out. SL_RESULT_OPERATION_TIMEOUT! "); + } else { + RCLCPP_ERROR(this->get_logger(),"Error, unexpected error, code: %x",op_result); + } + return false; + } + + // print out the device serial number, firmware and hardware version number.. + char sn_str[37] = {'\0'}; + for (int pos = 0; pos < 16 ;++pos) { + sprintf(sn_str + (pos * 2),"%02X", devinfo.serialnum[pos]); + } + RCLCPP_INFO(this->get_logger(),"SLLidar S/N: %s",sn_str); + RCLCPP_INFO(this->get_logger(),"Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF); + RCLCPP_INFO(this->get_logger(),"Hardware Rev: %d",(int)devinfo.hardware_version); + return true; + } + + bool checkSLLIDARHealth(ILidarDriver * drv) + { + sl_result op_result; + sl_lidar_response_device_health_t healthinfo; + op_result = drv->getHealth(healthinfo); + if (SL_IS_OK(op_result)) { + RCLCPP_INFO(this->get_logger(),"SLLidar health status : %d", healthinfo.status); + switch (healthinfo.status) { + case SL_LIDAR_STATUS_OK: + RCLCPP_INFO(this->get_logger(),"SLLidar health status : OK."); + return true; + case SL_LIDAR_STATUS_WARNING: + RCLCPP_INFO(this->get_logger(),"SLLidar health status : Warning."); + return true; + case SL_LIDAR_STATUS_ERROR: + RCLCPP_ERROR(this->get_logger(),"Error, SLLidar internal error detected. Please reboot the device to retry."); + return false; + default: + RCLCPP_ERROR(this->get_logger(),"Error, Unknown internal error detected. Please reboot the device to retry."); + return false; + + } + } else { + RCLCPP_ERROR(this->get_logger(),"Error, cannot retrieve SLLidar health code: %x", op_result); + return false; + } + } + + bool stop_motor(const std::shared_ptr req, + std::shared_ptr res) + { + (void)req; + (void)res; + + if(!drv) + return false; + + RCLCPP_DEBUG(this->get_logger(),"Stop motor"); + drv->setMotorSpeed(0); + return true; + } + + bool start_motor(const std::shared_ptr req, + std::shared_ptr res) + { + (void)req; + (void)res; + + if(!drv) + return false; + if(drv->isConnected()) + { + RCLCPP_DEBUG(this->get_logger(),"Start motor"); + sl_result ans=drv->setMotorSpeed(); + if (SL_IS_FAIL(ans)) { + RCLCPP_WARN(this->get_logger(), "Failed to start motor: %08x", ans); + return false; + } + + ans=drv->startScan(0,1); + if (SL_IS_FAIL(ans)) { + RCLCPP_WARN(this->get_logger(), "Failed to start scan: %08x", ans); + } + } else { + RCLCPP_INFO(this->get_logger(),"lost connection"); + return false; + } + + return true; + } + + static float getAngle(const sl_lidar_response_measurement_node_hq_t& node) + { + return node.angle_z_q14 * 90.f / 16384.f; + } + + void publish_scan(rclcpp::Publisher::SharedPtr& pub, + sl_lidar_response_measurement_node_hq_t *nodes, + size_t node_count, rclcpp::Time start, + double scan_time, bool inverted, + float angle_min, float angle_max, + float max_distance, + std::string frame_id) + { + static int scan_count = 0; + auto scan_msg = std::make_shared(); + + scan_msg->header.stamp = start; + scan_msg->header.frame_id = frame_id; + scan_count++; + + bool reversed = (angle_max > angle_min); + if ( reversed ) { + scan_msg->angle_min = M_PI - angle_max; + scan_msg->angle_max = M_PI - angle_min; + } else { + scan_msg->angle_min = M_PI - angle_min; + scan_msg->angle_max = M_PI - angle_max; + } + scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (double)(node_count-1); + + scan_msg->scan_time = scan_time; + scan_msg->time_increment = scan_time / (double)(node_count-1); + scan_msg->range_min = 0.05; + scan_msg->range_max = max_distance;//8.0; + + scan_msg->intensities.resize(node_count); + scan_msg->ranges.resize(node_count); + bool reverse_data = (!inverted && reversed) || (inverted && !reversed); + if (!reverse_data) { + for (size_t i = 0; i < node_count; i++) { + float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000; + if (read_value == 0.0) + scan_msg->ranges[i] = std::numeric_limits::infinity(); + else + scan_msg->ranges[i] = read_value; + scan_msg->intensities[i] = (float) (nodes[i].quality >> 2); + } + } else { + for (size_t i = 0; i < node_count; i++) { + float read_value = (float)nodes[i].dist_mm_q2/4.0f/1000; + if (read_value == 0.0) + scan_msg->ranges[node_count-1-i] = std::numeric_limits::infinity(); + else + scan_msg->ranges[node_count-1-i] = read_value; + scan_msg->intensities[node_count-1-i] = (float) (nodes[i].quality >> 2); + } + } + + pub->publish(*scan_msg); + } +public: + int work_loop() + { + init_param(); + int ver_major = SL_LIDAR_SDK_VERSION_MAJOR; + int ver_minor = SL_LIDAR_SDK_VERSION_MINOR; + int ver_patch = SL_LIDAR_SDK_VERSION_PATCH; + RCLCPP_INFO(this->get_logger(),"SLLidar running on ROS2 package SLLidar.ROS2 SDK Version:" ROS2VERSION ", SLLIDAR SDK Version:%d.%d.%d",ver_major,ver_minor,ver_patch); + + sl_result op_result; + + // create the driver instance + drv = *createLidarDriver(); + IChannel* _channel; + if(channel_type == "tcp"){ + _channel = *createTcpChannel(tcp_ip, tcp_port); + } + else if(channel_type == "udp"){ + _channel = *createUdpChannel(udp_ip, udp_port); + } + else{ + _channel = *createSerialPortChannel(serial_port, serial_baudrate); + } + if (SL_IS_FAIL((drv)->connect(_channel))) { + if(channel_type == "tcp"){ + RCLCPP_ERROR(this->get_logger(),"Error, cannot connect to the ip addr %s with the tcp port %s.",tcp_ip.c_str(),std::to_string(tcp_port).c_str()); + } + else if(channel_type == "udp"){ + RCLCPP_ERROR(this->get_logger(),"Error, cannot connect to the ip addr %s with the udp port %s.",udp_ip.c_str(),std::to_string(udp_port).c_str()); + } + else{ + RCLCPP_ERROR(this->get_logger(),"Error, cannot bind to the specified serial port %s.",serial_port.c_str()); + } + delete drv; + return -1; + } + + // get sllidar device info + if (!getSLLIDARDeviceInfo(drv)) { + return -1; + } + + // check health... + if (!checkSLLIDARHealth(drv)) { + return -1; + } + + stop_motor_service = this->create_service("stop_motor", + std::bind(&SLlidarNode::stop_motor,this,std::placeholders::_1,std::placeholders::_2)); + start_motor_service = this->create_service("start_motor", + std::bind(&SLlidarNode::start_motor,this,std::placeholders::_1,std::placeholders::_2)); + + drv->setMotorSpeed(); + + LidarScanMode current_scan_mode; + if (scan_mode.empty()) { + op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, ¤t_scan_mode); + } else { + std::vector allSupportedScanModes; + op_result = drv->getAllSupportedScanModes(allSupportedScanModes); + + if (SL_IS_OK(op_result)) { + sl_u16 selectedScanMode = sl_u16(-1); + for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { + if (iter->scan_mode == scan_mode) { + selectedScanMode = iter->id; + break; + } + } + + if (selectedScanMode == sl_u16(-1)) { + RCLCPP_ERROR(this->get_logger(),"scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str()); + for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { + RCLCPP_ERROR(this->get_logger(),"\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode, + iter->max_distance, (1000/iter->us_per_sample)); + } + op_result = SL_RESULT_OPERATION_FAIL; + } else { + op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, ¤t_scan_mode); + } + } + } + + if(SL_IS_OK(op_result)) + { + //default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us + int points_per_circle = (int)(1000*1000/current_scan_mode.us_per_sample/scan_frequency); + angle_compensate_multiple = points_per_circle/360.0 + 1; + if(angle_compensate_multiple < 1) + angle_compensate_multiple = 1.0; + max_distance = (float)current_scan_mode.max_distance; + RCLCPP_INFO(this->get_logger(),"current scan mode: %s, sample rate: %d Khz, max_distance: %.1f m, scan frequency:%.1f Hz, ", + current_scan_mode.scan_mode,(int)(1000/current_scan_mode.us_per_sample+0.5),max_distance, scan_frequency); + } + else + { + RCLCPP_ERROR(this->get_logger(),"Can not start scan: %08x!", op_result); + } + + rclcpp::Time start_scan_time; + rclcpp::Time end_scan_time; + double scan_duration; + while (rclcpp::ok() && !need_exit) { + sl_lidar_response_measurement_node_hq_t nodes[8192]; + size_t count = _countof(nodes); + + start_scan_time = this->now(); + op_result = drv->grabScanDataHq(nodes, count); + end_scan_time = this->now(); + scan_duration = (end_scan_time - start_scan_time).seconds(); + + if (op_result == SL_RESULT_OK) { + op_result = drv->ascendScanData(nodes, count); + float angle_min = DEG2RAD(0.0f); + float angle_max = DEG2RAD(360.0f); + if (op_result == SL_RESULT_OK) { + if (angle_compensate) { + //const int angle_compensate_multiple = 1; + const int angle_compensate_nodes_count = 360*angle_compensate_multiple; + int angle_compensate_offset = 0; + auto angle_compensate_nodes = new sl_lidar_response_measurement_node_hq_t[angle_compensate_nodes_count]; + memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(sl_lidar_response_measurement_node_hq_t)); + + size_t i = 0, j = 0; + for( ; i < count; i++ ) { + if (nodes[i].dist_mm_q2 != 0) { + float angle = getAngle(nodes[i]); + int angle_value = (int)(angle * angle_compensate_multiple); + if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value; + for (j = 0; j < angle_compensate_multiple; j++) { + int angle_compensate_nodes_index = angle_value-angle_compensate_offset + j; + if(angle_compensate_nodes_index >= angle_compensate_nodes_count) + angle_compensate_nodes_index = angle_compensate_nodes_count - 1; + angle_compensate_nodes[angle_compensate_nodes_index] = nodes[i]; + } + } + } + + publish_scan(scan_pub, angle_compensate_nodes, angle_compensate_nodes_count, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, max_distance, + frame_id); + + if (angle_compensate_nodes) { + delete[] angle_compensate_nodes; + angle_compensate_nodes = nullptr; + } + } else { + int start_node = 0, end_node = 0; + int i = 0; + // find the first valid node and last valid node + while (nodes[i++].dist_mm_q2 == 0); + start_node = i-1; + i = count -1; + while (nodes[i--].dist_mm_q2 == 0); + end_node = i+1; + + angle_min = DEG2RAD(getAngle(nodes[start_node])); + angle_max = DEG2RAD(getAngle(nodes[end_node])); + + publish_scan(scan_pub, &nodes[start_node], end_node-start_node +1, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, max_distance, + frame_id); + } + } else if (op_result == SL_RESULT_OPERATION_FAIL) { + // All the data is invalid, just publish them + float angle_min = DEG2RAD(0.0f); + float angle_max = DEG2RAD(359.0f); + publish_scan(scan_pub, nodes, count, + start_scan_time, scan_duration, inverted, + angle_min, angle_max, max_distance, + frame_id); + } + } + + rclcpp::spin_some(shared_from_this()); + } + + // done! + drv->setMotorSpeed(0); + drv->stop(); + RCLCPP_INFO(this->get_logger(),"Stop motor"); + + return 0; + } + + + private: + rclcpp::Publisher::SharedPtr scan_pub; + rclcpp::Service::SharedPtr start_motor_service; + rclcpp::Service::SharedPtr stop_motor_service; + + std::string channel_type; + std::string tcp_ip; + std::string udp_ip; + std::string serial_port; + int tcp_port = 20108; + int udp_port = 8089; + int serial_baudrate = 115200; + std::string frame_id; + bool inverted = false; + bool angle_compensate = true; + float max_distance = 8.0; + size_t angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree + std::string scan_mode; + float scan_frequency; + + ILidarDriver * drv; +}; + +void ExitHandler(int sig) +{ + (void)sig; + need_exit = true; +} + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto sllidar_node = std::make_shared(); + signal(SIGINT,ExitHandler); + int ret = sllidar_node->work_loop(); + rclcpp::shutdown(); + return ret; +} +