From 457f054053fbd5277ca31ab40275a1d0802bbbc0 Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Wed, 22 Apr 2026 11:51:45 +0000 Subject: [PATCH] Squashed 'lidar/sllidar_ros2/' content from commit 3430009 git-subtree-dir: lidar/sllidar_ros2 git-subtree-split: 34300099fadfc772965962dec837bf436706188f --- CMakeLists.txt | 81 + LICENSE | 24 + README.md | 182 ++ launch/sllidar_a1_launch.py | 71 + launch/sllidar_a2m12_launch .py | 71 + launch/sllidar_a2m7_launch.py | 71 + launch/sllidar_a2m8_launch.py | 71 + launch/sllidar_a3_launch.py | 72 + launch/sllidar_c1_launch.py | 71 + launch/sllidar_s1_launch.py | 64 + launch/sllidar_s1_tcp_launch.py | 66 + launch/sllidar_s2_launch.py | 71 + launch/sllidar_s2e_launch.py | 73 + launch/sllidar_s3_launch.py | 71 + launch/sllidar_t1_launch.py | 72 + launch/view_sllidar_a1_launch.py | 83 + launch/view_sllidar_a2m12_launch.py | 83 + launch/view_sllidar_a2m7_launch.py | 83 + launch/view_sllidar_a2m8_launch.py | 83 + launch/view_sllidar_a3_launch.py | 83 + launch/view_sllidar_c1_launch.py | 85 + launch/view_sllidar_s1_launch.py | 77 + launch/view_sllidar_s1_tcp_launch.py | 78 + launch/view_sllidar_s2_launch.py | 85 + launch/view_sllidar_s2e_launch.py | 85 + launch/view_sllidar_s3_launch.py | 85 + launch/view_sllidar_t1_launch.py | 84 + package.xml | 18 + rplidar_A1.png | Bin 0 -> 33994 bytes rplidar_A2.png | Bin 0 -> 47081 bytes rviz/sllidar_ros2.rviz | 159 ++ scripts/create_udev_rules.sh | 13 + scripts/delete_udev_rules.sh | 11 + scripts/rplidar.rules | 4 + sdk/Makefile | 77 + sdk/include/rplidar.h | 45 + sdk/include/rplidar_cmd.h | 215 +++ sdk/include/rplidar_driver.h | 247 +++ sdk/include/rplidar_protocol.h | 61 + sdk/include/rptypes.h | 116 ++ sdk/include/sl_crc.h | 43 + sdk/include/sl_lidar.h | 47 + sdk/include/sl_lidar_cmd.h | 388 ++++ sdk/include/sl_lidar_driver.h | 569 ++++++ sdk/include/sl_lidar_driver_impl.h | 157 ++ sdk/include/sl_lidar_protocol.h | 85 + sdk/include/sl_types.h | 83 + sdk/src/arch/linux/arch_linux.h | 64 + sdk/src/arch/linux/net_serial.cpp | 475 +++++ sdk/src/arch/linux/net_serial.h | 90 + sdk/src/arch/linux/net_socket.cpp | 893 +++++++++ sdk/src/arch/linux/thread.hpp | 185 ++ sdk/src/arch/linux/timer.cpp | 52 + sdk/src/arch/linux/timer.h | 59 + sdk/src/arch/macOS/arch_macOS.h | 66 + sdk/src/arch/macOS/net_serial.cpp | 346 ++++ sdk/src/arch/macOS/net_serial.h | 84 + sdk/src/arch/macOS/net_socket.cpp | 899 +++++++++ sdk/src/arch/macOS/thread.hpp | 79 + sdk/src/arch/macOS/timer.cpp | 54 + sdk/src/arch/macOS/timer.h | 58 + sdk/src/arch/win32/arch_win32.h | 66 + sdk/src/arch/win32/net_serial.cpp | 367 ++++ sdk/src/arch/win32/net_serial.h | 86 + sdk/src/arch/win32/net_socket.cpp | 945 +++++++++ sdk/src/arch/win32/timer.cpp | 72 + sdk/src/arch/win32/timer.h | 49 + sdk/src/arch/win32/winthread.hpp | 144 ++ .../dataunpacker/dataunnpacker_commondef.h | 60 + sdk/src/dataunpacker/dataunnpacker_internal.h | 74 + sdk/src/dataunpacker/dataunpacker.cpp | 259 +++ sdk/src/dataunpacker/dataunpacker.h | 93 + sdk/src/dataunpacker/dataupacker_namespace.h | 5 + .../unpacker/handler_capsules.cpp | 1054 ++++++++++ .../dataunpacker/unpacker/handler_capsules.h | 149 ++ .../dataunpacker/unpacker/handler_hqnode.cpp | 192 ++ .../dataunpacker/unpacker/handler_hqnode.h | 63 + .../unpacker/handler_normalnode.cpp | 159 ++ .../unpacker/handler_normalnode.h | 63 + sdk/src/hal/abs_rxtx.h | 88 + sdk/src/hal/assert.h | 18 + sdk/src/hal/byteops.h | 94 + sdk/src/hal/byteorder.h | 112 ++ sdk/src/hal/event.h | 206 ++ sdk/src/hal/locker.h | 205 ++ sdk/src/hal/socket.h | 149 ++ sdk/src/hal/thread.cpp | 48 + sdk/src/hal/thread.h | 94 + sdk/src/hal/types.h | 119 ++ sdk/src/hal/util.h | 67 + sdk/src/hal/waiter.h | 44 + sdk/src/rplidar_driver.cpp | 199 ++ sdk/src/sdkcommon.h | 51 + sdk/src/sl_async_transceiver.cpp | 412 ++++ sdk/src/sl_async_transceiver.h | 167 ++ sdk/src/sl_crc.cpp | 102 + sdk/src/sl_lidar_driver.cpp | 1702 +++++++++++++++++ sdk/src/sl_lidarprotocol_codec.cpp | 238 +++ sdk/src/sl_lidarprotocol_codec.h | 88 + sdk/src/sl_serial_channel.cpp | 146 ++ sdk/src/sl_tcp_channel.cpp | 124 ++ sdk/src/sl_udp_channel.cpp | 129 ++ src/sllidar_client.cpp | 43 + src/sllidar_node.cpp | 483 +++++ 104 files changed, 16895 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 README.md create mode 100644 launch/sllidar_a1_launch.py create mode 100644 launch/sllidar_a2m12_launch .py create mode 100644 launch/sllidar_a2m7_launch.py create mode 100644 launch/sllidar_a2m8_launch.py create mode 100644 launch/sllidar_a3_launch.py create mode 100644 launch/sllidar_c1_launch.py create mode 100644 launch/sllidar_s1_launch.py create mode 100644 launch/sllidar_s1_tcp_launch.py create mode 100644 launch/sllidar_s2_launch.py create mode 100644 launch/sllidar_s2e_launch.py create mode 100644 launch/sllidar_s3_launch.py create mode 100644 launch/sllidar_t1_launch.py create mode 100644 launch/view_sllidar_a1_launch.py create mode 100644 launch/view_sllidar_a2m12_launch.py create mode 100644 launch/view_sllidar_a2m7_launch.py create mode 100644 launch/view_sllidar_a2m8_launch.py create mode 100644 launch/view_sllidar_a3_launch.py create mode 100644 launch/view_sllidar_c1_launch.py create mode 100644 launch/view_sllidar_s1_launch.py create mode 100644 launch/view_sllidar_s1_tcp_launch.py create mode 100644 launch/view_sllidar_s2_launch.py create mode 100644 launch/view_sllidar_s2e_launch.py create mode 100644 launch/view_sllidar_s3_launch.py create mode 100644 launch/view_sllidar_t1_launch.py create mode 100644 package.xml create mode 100644 rplidar_A1.png create mode 100644 rplidar_A2.png create mode 100644 rviz/sllidar_ros2.rviz create mode 100644 scripts/create_udev_rules.sh create mode 100644 scripts/delete_udev_rules.sh create mode 100644 scripts/rplidar.rules create mode 100644 sdk/Makefile create mode 100644 sdk/include/rplidar.h create mode 100644 sdk/include/rplidar_cmd.h create mode 100644 sdk/include/rplidar_driver.h create mode 100644 sdk/include/rplidar_protocol.h create mode 100644 sdk/include/rptypes.h create mode 100644 sdk/include/sl_crc.h create mode 100644 sdk/include/sl_lidar.h create mode 100644 sdk/include/sl_lidar_cmd.h create mode 100644 sdk/include/sl_lidar_driver.h create mode 100644 sdk/include/sl_lidar_driver_impl.h create mode 100644 sdk/include/sl_lidar_protocol.h create mode 100644 sdk/include/sl_types.h create mode 100644 sdk/src/arch/linux/arch_linux.h create mode 100644 sdk/src/arch/linux/net_serial.cpp create mode 100644 sdk/src/arch/linux/net_serial.h create mode 100644 sdk/src/arch/linux/net_socket.cpp create mode 100644 sdk/src/arch/linux/thread.hpp create mode 100644 sdk/src/arch/linux/timer.cpp create mode 100644 sdk/src/arch/linux/timer.h create mode 100644 sdk/src/arch/macOS/arch_macOS.h create mode 100644 sdk/src/arch/macOS/net_serial.cpp create mode 100644 sdk/src/arch/macOS/net_serial.h create mode 100644 sdk/src/arch/macOS/net_socket.cpp create mode 100644 sdk/src/arch/macOS/thread.hpp create mode 100644 sdk/src/arch/macOS/timer.cpp create mode 100644 sdk/src/arch/macOS/timer.h create mode 100644 sdk/src/arch/win32/arch_win32.h create mode 100644 sdk/src/arch/win32/net_serial.cpp create mode 100644 sdk/src/arch/win32/net_serial.h create mode 100644 sdk/src/arch/win32/net_socket.cpp create mode 100644 sdk/src/arch/win32/timer.cpp create mode 100644 sdk/src/arch/win32/timer.h create mode 100644 sdk/src/arch/win32/winthread.hpp create mode 100644 sdk/src/dataunpacker/dataunnpacker_commondef.h create mode 100644 sdk/src/dataunpacker/dataunnpacker_internal.h create mode 100644 sdk/src/dataunpacker/dataunpacker.cpp create mode 100644 sdk/src/dataunpacker/dataunpacker.h create mode 100644 sdk/src/dataunpacker/dataupacker_namespace.h create mode 100644 sdk/src/dataunpacker/unpacker/handler_capsules.cpp create mode 100644 sdk/src/dataunpacker/unpacker/handler_capsules.h create mode 100644 sdk/src/dataunpacker/unpacker/handler_hqnode.cpp create mode 100644 sdk/src/dataunpacker/unpacker/handler_hqnode.h create mode 100644 sdk/src/dataunpacker/unpacker/handler_normalnode.cpp create mode 100644 sdk/src/dataunpacker/unpacker/handler_normalnode.h create mode 100644 sdk/src/hal/abs_rxtx.h create mode 100644 sdk/src/hal/assert.h create mode 100644 sdk/src/hal/byteops.h create mode 100644 sdk/src/hal/byteorder.h create mode 100644 sdk/src/hal/event.h create mode 100644 sdk/src/hal/locker.h create mode 100644 sdk/src/hal/socket.h create mode 100644 sdk/src/hal/thread.cpp create mode 100644 sdk/src/hal/thread.h create mode 100644 sdk/src/hal/types.h create mode 100644 sdk/src/hal/util.h create mode 100644 sdk/src/hal/waiter.h create mode 100644 sdk/src/rplidar_driver.cpp create mode 100644 sdk/src/sdkcommon.h create mode 100644 sdk/src/sl_async_transceiver.cpp create mode 100644 sdk/src/sl_async_transceiver.h create mode 100644 sdk/src/sl_crc.cpp create mode 100644 sdk/src/sl_lidar_driver.cpp create mode 100644 sdk/src/sl_lidarprotocol_codec.cpp create mode 100644 sdk/src/sl_lidarprotocol_codec.h create mode 100644 sdk/src/sl_serial_channel.cpp create mode 100644 sdk/src/sl_tcp_channel.cpp create mode 100644 sdk/src/sl_udp_channel.cpp create mode 100644 src/sllidar_client.cpp create mode 100644 src/sllidar_node.cpp 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 0000000000000000000000000000000000000000..994def4d839d055383a94729b95a3bb63862b26c GIT binary patch literal 33994 zcmcF~1yh?{)NO%6aR~12PO;z?q_`D_;!@n*-2xPMC{Wzpio3g8ad)?y_q+G|2{$vD z$vipp%sKnm+H0*Hp`;*%f=Gb)>C-2aZ_*%@PoJQ2Azwy#ILMP!Q4O(ApU6Ia1Bt4+ zXP#z#F;M-!I`|>KLbZOB?JQ1T$FwZAOhzb*h3!BF6vAG7L5w{4E{dgQLJuE_Y)a`R zzU&pN(E4DTo^_F_s-1y)D9tx>GaI+N4|Ymez3H5E%DxdW`{cZ34-zss6Eb_K|Nd17>VggL-L?m!K;!;@ zXuvQSibAN_YX9th*04y>$_we7aV?&ne@}0(mbpSs2DGB^8|VIb#^6sXX06(Syo~?4 zLW9)gKcYnOtHbl|=T`E6LpbXFf1MP|N6&?PW*kyR)?985CKq4Y1$skKW?wSFJBgdZ z>r%s>w=DwB?L~H*e^0r5SS+QSot=+Pe!b25D37q@qj_f5CPvdHo#wwj36_t~Kk5G$ zw~~Ag8d`gAsUSDB zMi2j5t*CqIwwAv-KBu2Iy|yIU$bMTL%|?4`e|?vkXxSyA-&@tGtt$O11qr()V&bje z?erFol8Fh`^X-x9^IwzK5N(ts(M4ti)LeC#qx!o**uw+yFx39z`Lesl{86FDtsw2D z1UQ;R8(JUvRsCdg@ldrD+m!)~Y>$2Xd%55OXFo-uCn_%?w{s$|KR7r5j*feUY=Jap zohDnoipc;-BndE4SVR(QZU12KXsW1Sc(EM0ALX}4F|x2!_T1mrKw^YA2oDcmtV-zP z?ajc%NLVXRDx1PmK_=5c&dr@1q5gEkz-aQ{f||2bf?~iYOyhN;LGt>8_7gub%=P%T z?w3N#>ucsOn^B?r^L4AaMped;Q0v+IWTJ%Zu)N$ron?=!DSOD2GjJAvO48)L=y=d? zSSTW9a|uzF4N@^mQroNKpqZ1&v+bz0wnopY9p1;=wRr%*|_1_};ox_h@R!3Qz zria?XL1J$*m+>hnm^($@Nqo?Bq3TC0n(%A2Gq2ANoMXq|m6f$S{HB9ikwFZ%g8{|B z#PEcgTqx){=-)=tvApIJL}pp|(9a!HH@^?G@7wj=w_)=&bqA=p6eDa>W*sioxzz~t z#CiVPSQLi^1bW|SC${m@72W!_Y$KyP-fdOT1q%Pc*#aQlkRF-Xv&K@kCz{=L^cI>y zQlo3xX#6qkFIHi*B%O#Ak)wT z8*Yqszd1JWCMQwSBA-)?UcsM%NZk|&S;ftt9HK>VYCMY@5_AeAi*UHnGmxfM))7(F z#XzQ5+aMFvnmB^iB*=2l+lKCA2ewt3W1L8VNuXRU4lGfe^6<++(9uV`&{jy~+J_nJ_Sd=*PaOa!$H^3o-=wIEaij>*hZ8N1hu@ZGM zz9-Z<0^gf%`hoUJe)vxG0H49ix`V`9T`DT`n!R(D`^$TK(Bi!>9!CL(r!yjzTaWIA zTvo4JEg(ZV*dZ%x`;5%2`^y~0bzI~!-qh~k+T~p~kek&4^u@e+Ig+4tIMY+4GSP5! zcz((GRHL2u9O?FiEmIe*UD)D$?k7o{ZeCBVaIC>b?Yf+EX;57dioTyO*+zyC5x2TG zOL$4`y&R$~HMWtNpIyCu`Kz108#nAC1|pkPHLLC!({W2k<}*zb8==)bYv24~U3Fu< zgt|V#Mivk)3)+j@c@~Hwvg{$kg_DcIKlv|R;-t5*18L4BJ zp@eyeon58&U(IPwReW|vFn<1iZpROe*Y+JGT`=;XU$%BQSrs?RI!HlKl@UtCs^0zJ z#zph>g*=Ee5OHdTH75)-cHz%Uq)J6`#Rtf#GGM9hIx*hw8EgQCQ4+&E-&Z}vC*p_2 zW^2Kt4Mr9!0RPCwz=lUoZkc!XPjk32cuTHZ8izR#O!=MxRtCOooTFNMh{_RXTuN5Fck(f?7Ar4|HJ%B+IA#n47k|GZ+jRmztMHeF*(&y%PnW(u zZa2YT2Zu*4Zp!K(Kp_%=5ptD=9Zru~tWiHuh+o?bkK^17j4d^eTEC?|u>0+JUAiK4 zS>+3bCiVTv))}>g|4Ah>w|Bq4*5ElB=&e`d(*KdC$5=%`47~Ow7?A$}@SIa0GZz_) zeWX2sV($Ab2@3WY0rA?OOOkj4*DDNIwqS(+AU6EG&;B5ksyYYd-1o9IF5XmQXfQL> zZ??F6eDz&>W^WA(-hS$~izie*|Hb1{niZX>%}DCkBw?p@B7&Cr_>xZ$g%*OiXz5s^ ztf8kR^kV2$5kvrt$?&58`sz&;F#fVl?=F`?T(s~nHb~e=v*}yv?IR-^HYws+xR|&2 z)oWan{l5XBQ-O}kZ0!+CCIL17_a^tE?9u2@l?&Y`?kJNo<~f%p^Skny8VW>|6Bbry zqFMh5m|7eJa&}z*RiQsf4Q5nEjNxPb)igbn=|#yT<@EBP@B7NrgwpQyn_o89!D$Qm=Fe7i;J2Q&p;98 zWPH^gWLE=%P`!m1DcD%aQ7#F@O5N^phaLMGRazEMrdgDdg#uGr%J#tm;t7~ZT+Ok+ zc)1i^N1E;L1=8VKEt>D@2eIwo4|XHj|NG*bY$VT%9HQy7y!fBPmb|IY=T&j8?kHyO zvrClDeYBdLJJw070$inS8!u<_YW^}?Vttn9Q_|c7PiEAOJ)~cJMSi;e%jaQOR<{l+ z>NV?{Y@k*@*YXdJ#-nUZoW(0vQMTyQ)(REcIM3rxXMb^f{kuoycoQX)qSrwjYrdha zPJUbw91p{H+);EBDhCA_!hUo@7Nb7NJNb2_cQ0JB zHnQGu<~>f%@1cfGhrChpG|c4teNC4)u9coreg8@#wbF{%`95qz_Kx-!QV#`dZaeb- z!0Y_uQLj6a|B^AK^f-})bWmeV*h+t=;0{7fCRG#*55wLdnS)mUeT<^ulrGe?Q@IsZ zL7HzYf+k1EhlCMtiqhB-{o`RQjI}28kMPwdfz7Dpni?Z=Tlu3B35`%Jg`VaU=sT`U z-o^`4o~6FY_%g+{L!yDRl=&@UeHd(x^;$Xn6+*vk@5bO$n^eu+?$2V*k)L*}trAjl z$2`Pt|Ejhxb~*!b&)MWlnFd9IP)8z_ti>d=K*e82kWd@^+PAb(%O z5>Lr{_dg`sV}Llcu-q4~1gCMS`V70<^lhNV5a7T`htXpu^$s1Vmx9sXRqm+iYL zBLyGj!cWRIyiUBOw{Lj~O;epxCk$?!o=UW*K=+oJ$v=8PHOu{A%qTZ#Q<_{?yl3EZ zg|#Q9N>ykG`N82d;%G+Qr9F^Z$;!+*mVRhVw!g}2pIw%2h@(Jzb>{MPdp%ps)(~}p zR1c0&*2@|3*mM`i={SlZ>sm3!Tkb+Ii%zD_NTa{@LlA9IDA|AahnHtagb`@?2W0b1c#Yq>njM0x$ILZXriG7U`)$xBAw zIk;0ZWyA|4`q4uLI+E7(MM5Pi{-JkPRT?w_ty?4JeJoL7Wmq<5K&Z`K=qSgfod*=w ze@D~njCy{vknOd->qG1uybq8ja=G}OkMQ6jrO9zJN2pO5SmPG}{aE!8=o70;v zW2pRO>kOZDhrHe*uQ`SGVOB@-reU7S(jq`x|L8jU6uSK*$+!8gZpX7m*GV40BJe#S zy{NCIjklTL$ESwm!LM9yMCDnR=fQSMM0pZA!Zfm_e)8_TFWV*W9C3<`ZzoHRSVQj6 zJAWp`cZ`HH*~O|B(6Dp1S-pRlPH?-`+{S+?Se^NWJolR+AaZ@JX+W)MJO&(hchYjH z*8PUe`~EjI)cBz`Z^j<#=lL#<8KsaQL4}TU*LbR|X3a?kfsJC5@%FNVhL%B~j7@rg z?05)65IP;FB^`F&$Hb;u>*+`Z@HXoayOG7OPE0KmL^tD=lQ%Z&r3h=iRwjG|%cEqW z%r741?nLlKI}6pZ5C^dVXP*9Tf}J?=C+v9YS1=QqYM0`J^6*v z{9jo6TBqLNzpUzBow2YF;tHZPHG%3~-3$1XBpKFybz67|VTkA17>rtVHQbSW8 z!rfZv$7-)L6XZS>&^cNHFxyHomR&op5rHj5Dt<))wyjaou!(rSu-0E&huu7YF&Q=W z`92PY|FYJcF26;<7aq_U?`=HU(Gdp%{AuXFt0M)UJCV(Tp&d%I+0j$kc*vFZqdZfjy7ZC1izEpW;-kZmV*n2H-$%+7_34*!oS>Flv1s*U3h<(a@ z1k&0+MY(y0)@M?+^O~`EuWRG))C1uPR;XJ>#X@V z!^Vpa6`vn=s>b)H%W~TXlQ}<*4z{adfxuNzB z%;1p9KLxzK&oM6wQbp@xK2rvAj!&EzAJhgH3fpFk4uDIehQ!1`V+0Qvl^n9+v(Ckh zNR?kL>_s0#;FG8Q6&PaV6eJ|~gUDcXnAdbq*%<0Mvhk+k<*h3iG`kbS!C*-P12Q5a zB6I)%ik6nPW3uG(bhQcXWU*>+clZx!hZAE7dRwJw&)~P{nk-#1?t>klKMFGH(13VF zO&Tk-K^%9xp2f=>_<=Er{J8j>Ks$ZyAhN1PCSZW%h*so#M|_OT-|^rw!J2{%Ji&+f zct(Vy>2hmvGhaS^(mQtTmtcaYQ=m&>IZ~wpv|9GOLm2znwPNALk4t?2O}-AxI3FHr zeXlbvCcRdyoBi?LFQnc|t(n@wJ}>vC(AfC+%l^!}_R->UBYI35@ASV_&&h`JQ8uzK zh243dwzY5Z$39MAmbCe{ig>tdUhSnSPy3P%XWgAn4|WF%mWUN!evif%UEY8v*@|a1 z1q8Pm7-C&G?kUtSK1lY(0A}EH7}CYAXFhoa;^KKcB^P^~Wqe0B`Q(QUuUJ99n{l3@ zu`wpSzwTE?_pf^?Ke(mafJ8fay(kp$n>NpAzY{KUgZ$9EflvH2LWxdpv7(hsVh?fe z=;&AUOe6iFl(G)b3*_be^`EtkP?RkW=E!sTX~M?)@Na_T<=3MEkD6# z7LtH=|L*>q1+f=@T24JNhn9}l@MD`!grxIbV`&`mSy?eKUr?(}=eU|2b|lT-IKM0s z7Cq(8|8&^D2fS9BqGE6uO}DKpwR4?20asg6Dk(LvM1P*>3cORIcTKE7+5K{{@NHf{ zSlH<*tSCI0K|lX%LK@T>J(zsCY5#tHH8W|kAGChJZE*MZ0zj~Y$ssinno)mkGMG6K zt4w_DSaCefm_|pfK9$Mqv~&{)i?qbi5YZN)*_Ki!LgQTo+PF5lUo0UerrsD>xF2h^-&D5t01na}FV_(jjsb!OM_1auf*2_JqtE6L6k$FuRP^_tYetz#M$NbjD_E16U(A!Aq>ApF5 z{lPMz7rSw|z5;91;j0@I+^OGF&()pEapB>7& zH8}`akn!_Yb+E(5@qGpOKGFulD`DL;|M16_x<-H0u3pWT(i=-b_zHR!PS z7eh#|t!pqDF^*_@Rj8Nn$GJpuE;{Z9)oM-?=wdr-i$aZTZ$PwMuLgB{I2ryN3%N)V zM?_mGF|2}iku55?bu1epBLc8aTel*}74T;l;R-=QW#4BF+!okeFC7ebZ*b#9=j`We z%j`h^l_&!Lf!$__Ngul*;%3=A=cr>5KQgsPjkqy}+2`>r$F|ys=X)qBvCO!vo`HdZ zI8UJ{5I?>mqxCxZ{A9hRRV-xXaIo!p4_z;%2=`b4RJQcuWY;w7fvQ!|mde1|^hd_c zPFX7Jd)S3bO<`26i^hULnHBbJy=_%!`(#U{j&OcOr9{=o@kH@$(*%Eq3eQ1DOY5eh zS(T?frc>UoSviS5+I(QcMX zip!gh9)wZV1tC9=8qe9PXsdWi z<56O1YXchxtNcUGkD}uN?-}KX^*eDB!SP;lAfeelO`f7Bc$NUexW*W&}!G|1~14I zTU7Oko6mw2>v zI^VtVW-ZTv@H)~mA&!9)W!jdao)Lrr>Jc_ysNof=ihe|Y6PN1l(Y-y4A%!O9@Z5{| z=$akIrL`mK+Q(#+rQD(0n0U81SuLy_Kj8ct%^-pxk@mfO*pj8%dg;!!nzL`OObO2o zMnfU1Z&>xfflmSM`-D#cqgrumt~Wb_1geqkp6KZ=EO(@#83JPoKUJ*&6%E#L1tYpv zw>E`{vJ4dq@hh?H!*1>yhn;~~6&c$X-EP2ZEjtFudV8Gb6R@#TFJkjuD00Xf=~9ka7i+0&U!jEIUbdaX<&64g zkB07BS|PDX?jTLg2J%ZMo8g&bO<4<*HAz

@%L5mT#!M_EJC{q$`fhohgkY_4}eR zfdPdm44!SOQ=-xk5oh|43R$t3|GJB#>`)_8!vr7BCQXJ@oR|3h7lC9#Y%?M4)kAIn zl07>v3va z8G+U|g&J7^G&(s16t&@Fu&kBz1ab)7dbOpv!*rfL!EN(A#;?B z&S{cuqJuz6LNA&UW1sh{*=9xaRlf4zz3mvVSmmMPs3$e2j+Jjsx8i2MSHlLrzY=UR zKYmtViDI*IM~}-6gl@sJ@d6Ll(-V_(`Jo722eDaf%jo*c~+?kcjAvL8M}z z9s_aH@n_?HB)>p?M&0`V<) zZ+t=lx9wQ_p)WU=7)#o`6iaO$uos-T$hJ^Plc8Pf_L-jIhsYXM99Tzv)xY?ZgwB?Wpnf?!+9KmDvBZf|G6-^9}F?r)>!b0+jp*+`3a z1x96=SCYqDBx{P5<7aWKz%7ekZ*f=2Pku^00FizB6BJW~%ha$eJ;CgXe9VcsyKiN< zbAr~#50a~7G@bkO&&lnHq*Hc>e0&`S;Yfv9He zK;Iaei+M49tPrGcjfYntFTti@Z5nagMjZ7Cl431?r*);`U7)KAkMDY&KlLlox-nul z@hx0?=G*b!qY5Fd9BM{@LV%#d`2@gPumSE#0y|3EF`Zk|q1)koZ(ievY3zPvSr)7( zCS#YZ44+v*&Bb>AA;+DE0Jkx*dZnYNdWKrL@I@4T-AlLP^I-(1S+atK@cDP-)hEHf zkM7L+_p$wz_Qauo-w{30Z|KjjZJG4Y!O3-P28}zW){mQC`w$zBnRsKu#2H;Bu>AaluFwK{_Jbc(4cp{}VgdY>m#x@Ass};SQEj^_x>Vj!Xk; zhzu8i6w12KnWD5NR?|P+*vsM67N@Qmv=O@2_Wn&aaelbuyf7yD9je? zJmL+?UJH0zbR=(V3aPNNx+Pgd*}y^6eLbTbdw!=0Enf*)KErfy{UFy>P!ltu4ppZQ z)*1YJNO=6^C}^9)W`*l?)ekjdm%}Lw<7dVK9$PKiYemQ@nqrb8$!b%MSb7N7f*69g zSB%(4vZ#@@})cMQC=RKAtvNgfdw*~ZBArU+I+$_ zXa+?mw{9*s(7xHOa?b3d%~b7X!sW%UC^606ff1sfu&0f&U^<=OLvkij$%cx(of=Sx z*f`a7btXo6-&&)s;czj($)fye5QPEIi0DAUH{6VL^PmK?Ty4B>GB&q*4Ze> zCBPI;p`vo*ctTrmZ=hiQ3HwdfgbvM5A-4CGt6&r8FU+Gaz9lQmUaH?$^r@5d5h{$u zF08KykrmnNOKcM%9Ma8-28+VvH%VPjEKDMTfvl{Y2@qJDt3K0pk{gD!RS@OMm#LYw z_IhUNHNO*F6+Ab9ucv)i;*tT3`4WVLF5d~!$T;`8@7?DP3FReA6=J&fZc^e+55QXn zv0nfd(5X>p20t1%8}6i*oa&|~3sKrf52NEW;by|_C&iYe%Ut!+(zLqNOL1L4WeLD& zG`$CtHm!%99JaV$gfvRJb?)JqVKVMmWwtossaVm<`TY|;C2$_a*sYhi?> zqJ4F7UyU~gJ_sqU4YV0Hp1%c*!6aXUn;19{J4TcRMUKFUqUo$Ip!im-9~YYPaR4&E z9ek{{Wm~LQatRzKn{34w926J~gD2PO*M|Pr3t-Q+&jbj@akrTxs=v8VKY3H0#?T1| z$bIfvtiaYiT{o~h3vZs2E*EY^ws~u%;M?Q&Y=n?+syOMGwsKVmsnno4J3IasxJeY> zC{yDtPQ7L%@T?qrkk28o+v?@YAjN8sVDy=p5LL)f8G#kZ2l5A|2_K!w*7FK7E9~vYfWqS;e=D&XQ$9B!wgx*6OGE8gArYkKiDQ4Agqi zi}7m7=5!BruJ(D;^^QUlXIpW4p5G=g=z`~wSmJzrUF}@ zzuwGF=gVtm-3wOV;F;_e8q8jOXX2X(4ObrYKZpCtTlAN$`BNIwBNe*>{@7tI=O~rx zMTrRzd)fORkG1gImuu5yyiGR(nmZp@qjlJRL<7bt-*^y2#W*RF zChz@^#(XqRIMy935~%OHuhsieLu5-TudhjNeBaMMINdP6v-_vuoTn56NmhX462;Y6 z`~c1&on#Gma61)9P=O3P^Ipq|!(8>U_~eW+vq@UBzS>3ScPTS{ z#Md<);}QSGEf8P!XgmZ%*8ET+n=~J0EL@ndok){jM*MFf;XQ?{#ZP9Nlpz`8lN z?khT=f(yM0*1tu;8;9);gyhPQJ5|1CzZ$3QG|9(H`lClyAgC7ChO)db$zuY$if6 zZCz9`$0FN-M#hp*@j=wNm46JjX1grGeZfBi@a)K$8=2U_Ol>)+-s!oRfy zk&wd-XSpkeOJF2q3IEE(9%Raas0Y@6$6pfuhUDnT29bs>ApEBHE~nzI_KvGgj6yDI zP2K-NS~NY?KbS078q}f`tj^50<$3C{Z*=2i1#4&S*Ual)t98Md6(!h`(B#W@)8VAV z8e9)=8g%?5!8L}Ht3swMV-SAO-MmLb@|%`;=jeB@PRh=hFZu-ULb`<{aKdnd z_3ZAvaL0}}!k2b$gu;v(eaQN@5C|cM>q)#n>X@s>xQzVJ`g@MffV|p(r3u3HdjGw5 zG1)8P(9o|*Dlm;;`GQXVyY5I&T8g2z*k}J>pew1EL-T!7LwXo2Rn)cNZahY7U+=_q z$Zk(7YNRy%&G=a~O> zk^=_FAHGm9x-VemosX*a^}NJrQl&jvy0^c<%MsqQ6fA^R?Bq_D)_nBfSPQnK$3XP4 zuid#X2dP5sBe2qz{M5KN z-CS1l&aE~LqFOi&I+Cr)gSi274f~-(abaqzM-Kct4g}oQ4vA&21ga7up9gxK&rs+N z$A|`KPizP?-Fk*@Dlwe_^A!enIEhVUP->CBqo*NlnNW&R%7p46cDy?we%&%S>0!!C z;JG)RUf?a?T88M7#xPH2G1X1XOe2H)mq|+=X2gB_7y=wyT*;L3)`m-Sd}%Eu#U?J?lGTZ*!@+IIE;#zFi9mR+{REDWQJ; z4#+a_#HA_RXB zKLL8ga_CTac(CR=-0mlo_y1(FJ90m};MqbZ5(tMRrnz)35uEkeBoO-#FwvI zO(^=I{)jfZ+F1M-EEUhWNp5Wnsweh9mu>??p|`A6!ve2)TuTj0be;dqYLBh3+jrAW z)6C7Jqj|~S_;y^vQno(V1lO-TyHTs0#G4U6nm#N7U95!p;O@I_Y2U4|u3B(>lC^xy z?MMTwtk7{u!P9IOnMX}FKZ?KNAGL`ZBYV)H4jxXWy|JSCk*@26iISn$-f|zD6NTtf zmo2(Z9X6Itn`@2`@=e#ft>#&eg)Jah$3i0UHih7%q1Fws;-KNgrroE@DoQFWR_p3zm@1eLUGNkl%kwWFXjvqX7By6)=H{rm{Ydmj?KBu`+XlxEH?G`t)W=XRT$@O(lEr&$9DRPrbXq}- z>>6$4JTxTRzq}g8Zn{N?x9G*d7}UKzhbwsmpn^v7o_@yPcP;yN*}eF~FXr^ry8V8| zs&KRz&j%u-Kunl@WmSKl<0^P#0E{*08>kq~OWYGo@LZyK3`SX8F_QU}JkN5N88{e> zLo!CG4&>Ts5r{e>XQWKJzbU9=bC4SMa zZQ`^=GY(|OzsgwD41C}2H2#yg+T1P%!U2@2c*@EvHO65z=_A_$7}_spJJWj*5R=tX zTr8oGW9eSMH1-R-zl?GD=}5D|97+(bYC2K@Rx3?JmZj?q7eDiz&lattC&Lvb zY4BswBZhy{nlDX!NLvlNx@lAdWO$;9w+cn1QJNDL)^jm0R(o!WIisBqj+pC2oY+w? zhuyFdWPO_63i{=c(_pn&YkJXsqX5$ssj3s1xbg)@$>GRK)!%Fw73CDXDCrXj4iF?~ z!|V~$tZU)$n)nDw@;a!i$@vE1LXgBa0_Fy)_v+~DR~TIg!6!G+0}noc1q_`a(xBcF z=9r(zbQ~O*%sP!GRk8y<5MT7bXUpYUQvu{Q1V=sw+_Ihoz^QG-VsE=4{nv_UJO|5o z$bk`so}*yzRGfWRdv%C-D%2S;f9r7bCe@&3?>O;hX$M3F~Y6uGtK!DG3%o!9<=K;D|0 zF>S0N$II>lSiBk2%m!e0yxZ^fT0D-EnC1pVI^%_r9pDa;TKKb!6(z^X_`LIua2KR>$%+x z`O|)X!ieWW)m@5)AVqk#9(G`6tIB=)ZbS}{i1JrbPUr=XPjDN;qJjCD|7206e@eVq zuBCQX{VC|yQaJGop?&)2m;u;9cKO2&g>T=^_a%)anf~g3pxO@Vg%;(b{q?YPiRn8y zUvGOnZ@W-}e9mli3$h_&mWTTvWUh`DF7O!?!j$gGJwss6v(1NN_ko3Q#Lgsk{`T!= z%V0J*f!L<$PXc$K`?m2?Ai3gzN}nSW6K^C~Nsy3nbW8nU2wNiuP^ zSOED3&3ofbmz->2KsXq3#P33I!QfJ-eW(zzZYI(O?eUG-Pr>*cW@KDk*g3w>^f&pj z1YC&R+}z%kXhp+4gZ@CtJp~zZ9AdeyeOXJTR1xme+9_Sz&NCf$fjNT8ZRB}urrV+5rrccY>{NZB9!?Dxe4;ZLZwPk?>|Hf z6dVb|q4IeETR);#z+lxyXo}M(eWGWUR#;)%DP!jo2z}%0n=0hPCp#cJtI|I>Xgrq6 zo(goJSX(Pi@dNi-mP5)V@NgZ2Sg=w>4b0$FTQoXz2m-ji_;t0cN}->wL$RV?Szc_; zZw+4E-I*eITH5xNs22S;pUkeVt~MZF&dv_4BJA6qRCv7#%TVeK}yv?C`i;C-W_M?Apsf8b`JnMnD>*6(1W7q|AdQN&Lw?<78SOt}gQJv&0`Yj)38ml3WV0-@cdCHTC|5OM%O?`ygS^ z{(dzrg}9zA^#Yd{N_D20TDT+9?$8LMQGzNq12bIceFgt6JS2(2n0&yxuC3)e(W|y{&MaGd)j&AFau)=Axd`LVLDAmIu z9SshLJDVzz){Kf`MnlT;*AjF%dNLv8wWgwI=GX3yf(Iv51Uo-N?|&U-R-;Hbl^SQr zf$rb7H7nFY4>s$!be~%>>ZX5B)dYd$ifMQOqXPl(2-pm*BsM>cB%yxQA;mMQe$~io zNqqYmF7}l(RI6e`P=mT1>^?kVpCPnLViwK zf{kE!*Ru3j8dtI;m^33fOdt)h5>%}1!5?SNc%PPOO-pG*M`lEoz!QTGu}v!NIZx=*|T-tQBIPqTRhXk3-VOS52K9O4&RrauN#59y9 zqzy)g0wf}ZsA3?=P3SuE(Iqk%-4%zEl`Q%C=vptQDt+a31sUPgg$qQWH{Q4yRkmOp zQp*jNM5;0DVv;kE#bOzWt}Bj9iK#=a6(r-$45yqKj)`zg=SP@6;@JPfl z2XKF(%uB+${i4RV%W7_JwQ=m(%1=PR<(B^t?0*|yiGz%r3pYbSl5Mo0Fa?Ilrr(;V zNLH3-Uj|~~PfeW@(2Y?r#+A0RlQQF-a!7x?^)|BsE`KwSVxncx#X!25Vb22~XbzfU0iPWhb~j@Dp*9q~OOjnRDYirOYVZifbgy@fcZ zzx)q6sZ=fN7o2vbY$l^_3F^ISX+d5*uU%eOT6HzzZ}#w_ii`ctat z^EV=zd!IB-8PtZ48Aj3k`M7%|Enri~f8sDQZI6xd6lA5!BanoyBLjUo5V(y1D^L7@ zQSvVjv)5Dk4gBZ*=K03CW!dgSy9}8T(&Wi1P|)jz76&_WpT)u~jJmPt)$HBs$rloc z`}#&oCYoi`MFE5t!wK|DkH5rNf*O|6wOHrAGu*Lpv?RZL>01&c&}?u_+8DipDLnaMDN zTZh$8TL$>d2t=l+hmvCUEb@$#WfzmrLFkbef*2e(_Rs1JW8>pfA@B)pki(!;nf-Sq zkGYpDw{TJW>Z#!x_5Fg&Fu6FHcl(xt$KRsO7)mgn>Jn z0gM3(_tD?RB?OMVcT{es78c{3_nl+@jT9^zaY%p3mMJIk3WjS2#7}Qyc-YYb^73<# zGT5Hkcyzu`z2L`BWx^7sihj}(6(Y(kkD}Z<)XgJM;ATes(q!P|f5=O1g0=NijsRM) z0RChvhEe$l=){T-+ox@HaH31(FJ*-uZ=)I>0%EM0abV0-3LDz3iyKzxmMIu$!|ar7 zQhzfvv{l}5^2XxyQM8u%yn*+^%lGI;?dJ@^#+ldlNJF2OU04VjIgg`mALjz|ZoK;X zuH&nR^Z4qi|2e+fBHH$u=Gy77?z@hX;mDdevS@p*^e!(%O4QHe@GUs=&?D7SaM{w} z{OdYns<+27)-~s$3#Xp#?BGGtE@tLT=Pwf(2wq5=kZ`P_w3+N)ZX1TbcNW1l+SZGb zH$KS7qNR0|{X8ZXv5EQ`IaC1eXq)`_^VrJbVlyPX(h$PD54$N9(D^qM3f9b>YSO@- zyYAS(hv@m>Qb8XGa6w>NUzp~ z7n@>kJCmu$pi5QJisaA!u88G0H{VUT=tW+JP1E!;!=7=R=$_v|s_@PcwU3&B!_67$ z%Xst{Jh-&K?H;8R^3Sh`5`CD2fVnkR1EVQv)_CbIh?os82BQZli!<&fIowiY$_ZBk z(V7~AQbgSOmcQa=COgkKBloZTk{rZ0vqePFwm3K#D`#ubMQD%=n)+nUoin5~`@EIi#WaFz$IOga*BB6TEp2Bp1|;*06>y~qLiZmFIZoe!c8Lr z=z1qrU@}P2SA@Ai@WC?KL1nV7?F+%FKN%w zSPP1vx+0V&TvK=fLl$BO$8F4f#9`QTac@KI-F+t&{duO@N|KeBp;J&kTT?tQL7QR= ztVL$-SnZwcFxAVL+~(wWM-N00rS+T1c&<&$vjnZw5Jp&hW6IjX0f);C2ec@Kf4bgC zgrp8u(;3w-uliU+Snjr$CcqVxp8jGc3cdwvBgX#KVN_aMni|NZ+U)8FjZ2}hb(wUA zw|e3M;p&KDopOWfjR-Or+m6)Eb6MiZH+gk5z;efB612Se?8fA9 zv9Zuz9yI|(LdZb=S+0~qAwllD`as;6^_&G?hF%4JMbq~&Ld2-sgLua^z3h_L$-mNP zn10WldQx^iSC!AD#^T0^*J<5BD%hjyosf|!m%qyA*|i5I^}r4V)5S3^e^yNI_HW&& z>^J!xQ-w)*(c&QtBbu$^IB5H*^?^po@ICa0B9-A{1G0MO;M6Xb$u`73Tm7;oxum_a z1L4NC)UCW2W!-tTDY*qxOcmi#>`l{1EzT%p{4=m>N<8L;P{;9`wwD~$Tep*F--93m zG~uf*FmoRc66iq5t}eb7g#YydEK4(`j-DBrFhqq~?Hz?bMnfkEQZ^F{s!@!2PT+z1o3x5vo8zjoH-nr$3$YcOERz+Z z&;Be!Z8YLGKUSWyUU;h2Ejakowcd?18}aCDGryR7(7ny60g;^1ogJxBIw!^w>bxz_ zUNciyqa?ehd0Dx}yUpOoNYo?>jTJ)Y{w^hYX-f7T^u=_PJ01}l3riH`>SHe;S#A%> z0H?nGa~5M6kCDPGoDp-u_xP&*wVlDDU^VNJQL}c(2}Hfo_b9~r+eyg|CL8vD3W`lA zc1N(R{j(Gv3zks?hSr6d*KiVk!Pp)AllcQFj0#zGNQrp#jzD?<=c|tDr}F+pU3qja zb@n;_(OsR$x6EZ)9yRPn4Cg3c{D5U^8D~o}7(Q9XU-{ z*W6Q5v|xTe_6@Acq~>U<@6DaF0YkFgKmFgFk*Py_*hs)BgAr(!hptx8*6P&r;@y@j zTezw3do}q*MX)EPk%>|?^#6-SKYX#?l%^7MvlmF3k{TtnLehx@CS%4@c4B`j>y6m` z2t$Fr^kR+gfj2T)C8d=3dH#G8G$9}gq-u*pkdrs3I~T%0gMIy-_u%EP*fLZWcXtRd z#Rq2HL3$R;CaWfBT4%Pjgl5xw*gypW3NXZhvIBhj9J^HzZ)0_VoGZGE;WuiP%7&v| zj7x530uB2SQpz?It-}|!r^_olLvyQGI)7w<+d0A{24c_5#MuoLmBd?96K3pUaG_HSw+6S| zfoNoA(T2J98J?~VFMNoZ0T{ePs(m1{0qd-Pl2fV2C~kuJ2n_B2NS9m3V(wJ0M&D>$ z3cD!?mtKq=YBjq`Ma7Pelb!5d8V0-T<3^oOJc-L$3)l}^kP46ykq9WjP^IVc^ig5y z3~89W|K2g8Zy5UH$7o`?LTSVQx|pvid~#hnIJy=$9l7qr0e4-(5A2#tNyP&(`}BEm z*@1<66ie3MKnF6vbw;2Vya~78(^o+!%`)zxj;+a1^Ocpcht6l5+| z-++GY+~0`L6Zsaf({f#I-Xv{)N@}z&L(lqWJ~VH(>b>XCzS?z=S-*mc$GG`F8EZT}92?IS;j|kLX&z=8f>WGyCD|DZ&jMp04L_ZlM zR23g}uKCnSvz^n08SO8qX4HX*+S?$?V`AC&1Y>sq?%)pH%181O&%G$A;o|m}?S?-r zw`k1v`uE1y2Cpg;)K#bcC5KLs@W6 z+_LcF-XI{e*t}`H8aNKWJup+S!^xC-6u8|B<588-xfMWMbYWqdqUT*<;SQtXbvWZT z!R(0p1X)XyNSC_4hqH$U-N%d7hr^vhD-n;M+TglfIqK;}P&p;UuEJ%0JFaSkfE_fd zQAQl`>{E-ijk7u5Wp!zN+A5H;KJuO04&Ux`!!yVu5ZyHn_TOO&d_d2ULP$Lj|4E&c zYeP06iQeRO7vajU*hJvQzeU*r2P22yW_uW@3{a=$BU4}!!?r&yVmdJ5tMKuOx~ca7 zjs!FT^tJ&P^0=18JUbW2SDxZuZJ#mJhP_i2-8jA>hz`2{@JhY#{vtIP#B)Q4DmKs@ zTIt~bvBFcRsS&pj6j+1c6JQH`m|#;WHh^G`gpi1MlKWm|8?)m}b}w~!415}LBYsiM z@nF78FwaoHb$pn-Na=lm<#Aw@Q-@|dBc=`m`N8EDx+xZVp{%!H&JH(Lp1n%X0sg8* zSYYA7-STR>=Kd^RLkpfsluYL|%g+efx~ z6r2_V68FW*5C2Pfu9Fo$L`-R_gvD5$cI3x|eY^w|Hl+q5d2ytu=Y-^OvGVl;nx0h> zvg-a6G3uM%JITz7Qi#Z?F=>0f6{ohbGf0-NRQ9mx#aAt%>CgCG^2>910Xwj|SNy9P z3n}#$5mHf^f-}FYBOi3vhTKjv9901*OjGr6KQ$%C#lv$h zF6@-W2@E`=#pUs&kn~Ok!eZuJKH3+GpqgJ6T<9_wd>OKeqZDz=DR|F}q;q5t zDtMBX?6@C|?=~N;-@KaAws#)ksPzqvfiilRZpB4*yELd2fpI>9KScm8n_TF+9skDc zBx0}w`}>9d>w*ILhZq6|9nL=+o0^zij^#|)G8ZBCgkdN^5Jemt+I&!6&CTa{r*7d< zrKD%+eHDwhEc=CFo58W*rgyI`5jlUe%YuqEF`=Vl>}pYW{{HX;_j*Pds#rB2&^;f6aysD=*iA_;`y zeL8A43~e$kRUfA=ShdqoTR@9kc#jxNZYENu=Woj@DzTeQ5zNx3;tKv&_-@(fj+Zs9 z`eD6@C2fu}fVxU|-ML$R^0;VNZo7OQFQ12{BLvoDjmE#cFVvix<-BappCaT}JNdAv zz6`U66^i)+wy$OK*)lcx0RaJC_T1$mqHT>%6g0?`#m=ahmjQ)#xrTpC zspCU{vk=;HSh^}zv(h@YCFm9|d|mZ6YMCgyg76#ZS{Q?2yK5PPklC zZA?UNir8+|->zIHXR(#-)U`Y=E%7qn*jgf!+RZ5@wiOpOSIaf09*0t;%Zji|a78A< zKxraz0Dz6db~T`)g3A7r4a_H(?rsFcp<8eXuhkgTo*dJ7?aq`=ASd8Z*Q}>4TuZ2# z%G8rJrc-@Mn*e}}Q&@9uuTwMMu=dKn6&FWjN$AXXuFZnA?8A$TVa`yY`6DaO{TBxF znwu#&{Q8VD1HG611Af&e((o1C4XpQ)8#=tvFf%?+yu`0zH&Wx*3O0bmH@GOoOWQTx zR@_$@GB>%Mg=ad!`xQHn+h)iiUC(UyCV^&Vc+!rNHCbTx65Q**aD(pO$IHL`jw`4a zyd9`OX&jr4jaH9EaMSFCdv)NQYr&lP(RHGitV?zf5y{$`eh6P)l(+f*%9~2MQN29T z881)W0hzO-^C8sls`%Im`7;~;35DEcdWtzL{vEaQ{p^9<>OI!F4C?2#vYy)OTPP(} z(XKzLu$I=*keOOKX$vj;r2DT|DdSQC zHvYW2WC)`8hpOAP8gd-IKOX0yq*RKmL4boRW9JtQohPvPDeAB^5Lyl}9NvZUy+ zO(U6*7vpNd>d6+?pNgTWlu0Pb)D~E}T9$szmzM}r2=``og;)mTuNC9bb#9|10IAGJh?B?V2P!=#t z5;5XLrCpPauM+OCQF!nfI9jLdG!GA>9yYt`(;&ca>KODiY#0XPLc>YYX%jM25s^8a ztJRS*m?~vMKfheq(R3Kgo91qTDRKvt8Zyr>f~JWPH|L=-CC=3d@e-5QXKy`UHr%}i z=L)N44Q-weAMFBiYQ`Q+(ajnr8!7RDc7PyJl>$W|rV9OcD$TgV&ejg5V&cWdfXsJW zyiEFDxoBhpbxZ2klUteyT?V$=p2tI*ju#uM)!{nzH{OB*#hiE$^`Un0inW&WcuU8v z5MPorI`Hd&`Sh(GAoh<&&)mHBCUMsNet3GG6=lmE|1m}6{)E4=7m~u&RUFH}Hyx2b zJYSl(?hls8pSS3ldY8w6qI|!wA|~XTp}a7aePy3wwKDDp{N-HP;X7|`jCJXi(%hr^ z);N!%;@*R_#z$u;H~qlEoH+RWCL#xoQi@7U0+l~}vD`@$Z%R`f3jd#yq zFPFA*4C-N&tatR^u?q)?a857ZPcsk#wdbARk{gyK^Sw7|KRpM!VjGxOh4Ab3U=l#6 z-307Jk~pZzOUC=C&qTh?MBb3e?cT}}*{r?>NPZ6lCbdEgsyo^>We&dw)eNscyOzvb z;!J;kU7t>9C^~Rhl_ZXwOj^u&Y`g7vvpzr1be!ao^*XHWcX0sMMnw}x5N(5dm`Bwt zn~HH*#NXw8pL(?F%yFT3kOc=DH~XJn@~50I>D5|_?5kCB(-0@DEYzoVm+vZo4?CZ~ zvnzfoz~fUGt}pOcTJqxUEDli%An$peVka8=F_OujdN9gGE>_{- zxKss#7@sAG$o0pq$C%f|f;ygEpUUq1u3oTh_6nb@mO3B&<91$w6?Bm0r2e(3|MbPp z94zDj5h0TZd3fh7x7b++_{_~*5iqDg(zQBrEgo>l8ZE6;Ul(Z9-YHS89Z+;IDqsX@ zq=5LxP>$cOqgYI5YJ-{+gQ#O_{Y)a3D7_o&jsq_i;aQH(#_p=D;zQ0;4|L&+_*qvq zJ$F;OV%~&7XrB9Zsrel9TuWKZ^OeIn63zGN@kUkB0j-8U&-U&Qf%>q+Bcs=}w+_QE3Yza~0-Ux@cr z=R?W-xOVEk^BYkM_~X9LZN06;M}EM`Z2Ye|?k4nZPdd;~V%s6v>>poPk1gl60af&( z2VZ%*cf23eGvNuhc)*`G4m426_U#wD146t|2s>fkX#`rIFz+3ke*p-G!RIgD9v-{B z;j{#?LoLFYZ(3uAW>G$Be+J64mIz%IkGA$xM2y^m$o%Ahl0SPv*`FEc?XVKe2&P@w zbrdb5;zx*^F40(6If{!JQ+ifkagqlc@=lSlM6KgVv1bxrGJTKQBk!G`ePZ*t#Wr%3cC_nr0c&|*+Fh`R6c!UJs z)+WnFJH98|=4!e)lsi54-MHop#pgrAI4ySqH7fcRB_#E`(5>q|^|(Ku7yTthZPv|> z&%0{1Nb7Pl-(8pDy~)fsKdldMtw?;#v;ak|$(>%%Rx7`v8DTkNcf8TQWBa@#D7-eA z2DZHGb}~VFZOZJ`h<0#7@4DOolb92o@AeZN&jLDS z)s|c=BryjksZ(J!rYlKKl?L?>I$9_!LbWpCg<;D-@_maNe=(sAhiKo>`!01I}9YY=|2 z9=@+5NKF<`Ji--=?Q+iQd@Job?47+quOoP%>9u@EbKp>Y=2=oAx?=IIXS~M<^To#vna9#RPqkJ6)6nrp&>{*MM!%ubYBZmw2iGos}k z8^WGvkqdf}a;rME8Ms>GOo?N$w2Rb~+-a=_i6msWLx?&;s6sM9;xjg9MEHf(OAxr& zxP^tv_f)lKw~vgM1ySPZ6iG;k@e7oF4;48jS;^+b|326pB%fiQn1=B*Xq&g4SK1lgv}d)M#1py-O%Sl!GQE_?^T%$=Yb5e5s*2Zc^zcv-$B zASGICkE0$lZhBTmty(r~;QN`Z_A}%l{rm3ekA;kyU*~rLI&}=nmGgtR zs-gv5kNY0V=k10f+~jZf2|tG}EB-CFcXmK^XR3hr0d75dH~V$%3b?1NO$0^H-&7HX#hhhTHW?N7GX2hn ztE!E1v+Yjqt-nwl|G|6j!tK);$dPgaROyZXJZ{hF_FjBjz`02`%IKdP4PS%AA~7W1 z;^w0Xg}(cY2*Ga;BD|MB)1e^Xx6{a|%NMoX4S__9vW>^n-Kosui!U2DKWA^>vK z{C%4ySj?tFLBREe=e3vX%bK^d#LFbM{G$2Fqq8DTR&3Jr<9M1$&afUt97gIWG2f^- zEf4e@mB3EHn$-0Xc|6CW33SsA3yF_zG;|>-f>gF%`*dcbt~9*wUA*(C>=CfiB71gz z1QVQhPNS>eohHzBf1%>>b$?|rmtAg<**Qd9VSiP+kfTPulo1@_;4keq5%V<8H1)8@ zTmVDDzz`PIm~=$01gNdTG+P*7SHQ(rFYOdXCLJ?G{9Nm48yj?e&-d4mXlHMEQgZQ5 zM?o?&Dv3#?_l%K=R-cbusV>fT&=fTbh1pPe;C+Z^fy++Y60+OKI^EI#M&S1*0R>gv*FuTv}5!^ZcE{ zz_St8E8GZM7?Eqg9x{_7)9FK3lrt5pL`5%rD;Ye_D5Jigp&f>V zn?xaPR_G14WYh|s6e7A}@|t|~R;oQ)GMMahahrpmI{A9@&2zo`(u+s^k%QmmwtUIT z0nx+zJwR1ch+#rewg^6Ui|rCOc>sr7q&iVN$SC8sA5#e;F$Y^R?bLEYu}%4xOCA1B zfZ5XOydo^5Q<+wHB3xfbm4QUzZ|7|o`M7xaD-9Dz4?~HK6EpP1ym;SGFDRbV-AIjb zYwAJ*jcxjM97PlRbV{_~^nf6E`X05ETsoUq{JQIc7PGab#ds%{jg%NfpcM&hLB&xr z$JNjA@XL$=t;GzL)@Nxgf%u|iOItTNDp)X&c>?eAr{dM!8Jo0E&+>LrRB!)q zUKl$FClr#i#Z6vA9HG&6I%b4J;z5#vU(@nleT@y1v2o}&I@cLB-CXowe%_2cZzH)= zaGpkydDtuplOQ@77|yLM#gk<(Ti`BWod~9vmMzgEl9HInJC6G%-H9*+fdsDvf$DH) zTId$%!LwJDcB~>Vo1X)Syo5n1kJBFwp`G~&F~EJ8lmF8Krtb0Aj@8Z#N)V+XnoZy#qeCJf{wDm?BrMPr?ifDV0bav zWL9PmglR%&~PmL5ps zM|Iv#idd8`pn^d&$GHLowl8++Z~BW_ozhXj1YT$YqjDYtEt8lsH6Hrj|sJg zB96k12I1kALX@+j%fF0_jy#UP`mNh#B2eI#d;AN3#X-_zUieT|2feIXYOAKvSBGhy zqlhd~hEQ_wge^17U>2bh9kVi1{%VE?#mvy%*k%NuUsT%wtK^ zJ)ia#!7OA>hL$MZ>p0Ge^(4)C^W?I_xrT5VAKSFKrJFkmAKjQ{=Q;cLex}^2o21txI95A= zcDKWg>%x9FI?a?!mKxewb$tS6^gWVUE_s+y88pz7gMO@4Jgr@{voWK6(vP?%gh2^I zJLFW9=cv)@DJLla)0>J87xPTGm_MTm3jZS6$H2P&hXuI3S<6^o-&|unp)Nv0-h!WfpI=!ijAJqf``&K&XdvOP4dObW)`H! zuPD%V*+bhPPZlxP&4?Q?rJY@?n?g6LF9Nw4ohrY@QsQN0#A8-UKKGqtn-F@85~Y`x zEX5PpfCbC_YR87_JJ*Pza&JDPgo8cpVm3?vdgP3*`m!zis=fm5b*LgiJH79@I zf}MKDlqNW=asd6Hwumqf;ddrsm)liRt!y_EkpTYnF%v2X(EpRN)5z(wTJ=Z8+(00q zZjC?@;oyw{0J)h&Zc@64=RcD)X?EI4iVY2fHZb;8S~%UXLCjkEg9^i zA6TD8{tX>a0-I0@sv=$aHzcbh!g;4O_blx}tYW0X9ujTPI62a#_mha}(lPEU}0qdPw>9 zMXH16rx>~X#_K`j%O-S?dv*kxHeAb@Zwff1+a~S{6G?KwF!Ix7v*%s8Dd#|q1cGJ$ zhoomT8{~-A z@FPV46I-Akwa?Wy0T&_>TEdtMOo+*jerSS5DG(3N$%58WLeTtRS zb;7i~lR~q?bynPvCLRNR;Mx}J_hd{i&Pm_E&2xJ;=$Z2g;nkw-AK(*~y3cS-LKM@6 zRMav|CuP5f+RZ-G&hUHa#n(&RTtRe~cf833hz9ZhGZlH_~dH3XUQOu1--wyKdtRig;AzJYX z`qT=rx+tGtJ+>k>F&TnJIWjE=4+4UR7#5GDVeixu(?#Z)#jg1C)`fo2^Ea|ek}kSN zHNXd$IRO6aTYjyKRH(OmAacvbqBLEVGM}a-H zX9YU{&&L%?2#{PhUGs1xf-KEA*Xd(HyNy;mV}hmzf)@Ix@aNRjRJ2KA8D!v17k{1{ z=Y^%NfpyWy**vdv>h&gzS+NcDowwz-isoju+ThRKk2!&y@O8|5(UOOYpr^_Ps%}RD z%@9lqU`DlpA>J6L(z$Q#1W#?A}+R$O$cW_|58h5%Qy^~uuB&B-nup+vVp5hupxIr&Q@*o@MdioX&p%L%?T7e7WQ_#`jY<~}+)!rf);6{XYa#7s z%l`(oT|o;q{#pU>DgNxfbqU>?X*DrJi`9 z5-HLDVH@}%+F=D>^#Qycqw7f4;$$WlaJMgBxA7n63+;x;>y(5V7bpXFpS_ckhQQ`hQBwuo%LFi}Dxd~W1_a!N7$rNiG8C%eM%0HIPHY&{4Q9jEU34zs z(5Fc*jfahmW~0$goP?fnJcnGM*!dIGsT$PkcPXWoDK{(xmB)dl86G{g)P2W{kGQ+| z9wfYQ8!0gh3OXwmRyBHyT#w;EhO&|pQ?XkXBt<_dGhtg{p^?NWXuXdi_t^>Loe?Gl zolrVxHt+ygib8B+4nZJmOIM?lLk9hFH8?SbvOFqHf?!^7FKIS!Q%hA+BSN)XHe|tG z13XRg*|RSl%%5Em6_{WRvls~v=sgFb;6;#b8edE{o~>i#`O;HQB^d3S7sM&LtONks z3ja}$L>fkV7E8m>Oemz7mX0MnghFg8oLahSBsZd<*Bjq*(}A_OKQLn#be%k;&o{!c zTkA@=7l;1Hsax}Gcx<}W)a;Wn*ETI9pSV4|kL52Lfay8>L=M9%E z8Qw!y!x{d_M#Sh4Nr5}0W#u+n;E`^HIGfmLt`>@#ZW&Ax$&5(4MDOSkB(`*%{i$hU z78d^TAeISKV@4TV=tj5tNzX=1j*=iWn723L93PG&@P66!j}fAu>zD z6Y`mSwF!yF#Z{p&q@9S%eiU=1c6%!T$hI%&v z59ud?t}#hvJZT4IWla{SEUYmYr4(7QK#Mn(z|8&B0XAXk zeMHh|a<~`Xv(*hj$-M~M0X;K|zg}Mb6cZ=Rjuv`hFq5&4Z;+M7H*wFcJ))hS7V-TS zGXkssT#aLE00S8@G1a(ZS+5PLApW1?)|zzO(A9otE6bbo$$zYNrVV1MMJVL${XFkb zW*#l5nyRaWT5TF>rqXu<7H+}tGq{VA7Uw*sh9GGsePd_G&K+h!%B)pWd-#QKPXd`5 zV|)0eQ>L`^KTbfZVAWB@>V8iuJm$LLI_gh61pq`kL@JyYx}m9<5YYTI0~lCatH+W} zm@^c?E0t8si}PpTlzDdMMCG?W0m42|vX)v#6o?41O4TSJrp!X&$gekBO9 z>yQ#1iSnvSA2sO89w)J6Z4m85%``OSH1+i}BseY(Kiq*C9OHk(l4(*kn;?iuG#sQp zUMgskT!Vsvr6VXA7XG_b62is(JjyMg5PU@CuD7vFIcl04=Vy%&@E-)&YYUWblYxM-~T zZ6ZOLzZUTmyI_YQ8{hXts{RIyzSlN8X@v;B!J>`yqm>(G++jXMduzZ!A_E60+FDDO zqK;G@_R_9diQ%7^3PLhpZqr3YKTk@RiAs$m)s}4D1S4yiG9sq2^OG(nD^33#$8mw1 zL}H9Goi#7;P0cQRiZ=Yy0vqbPYM2B2o_bcX@aiJ!ch9WwsP+0Giz!XNlK@JZszsk_ z6yAEcQ*L{R>6-zv7gR&vU95u#UG?*JslmsE;C$!w#_1EU06p0aN_9Xrw55i)@LS5| z4se`;|BW*t4J!BHF3XKOuTic$+$`|cQmoPyUY6XmJI58E)*KTS{1^4-mn`OfN_wJ{ zm9)AHx;ioof}tGh>pl~ztF&-s3_z5bFW0n6obIMLxEwd6e|Yh@^mXA1dJ-}?DMKl0 zytI^ioaMBTm~1@RNe5=~*lxpWG%oky>-}cYMUh{6=X1tj%-8Qo$DeF8i)ExL-`GFY zn77pr+uW!h43ODqWtCTtFLBr&$u6Ky@ftFTIH;14$EM7VPQX`}?1;c)wKLxM{!F>R!CXu;>Jfp7f6;^Ntb!8=VZk5f$VQ-7P)+5()6Vyo0Z&{3#w zzgauV$tTm)5e)pC8Qfq#C&P5Aef!cMF72dzn(89937OUjQU?$dTG2rfk#PM4tK`Dj zF)_Q?=rod*`mFsC)y?K-o+B<4h%;T$=adC#O6#t}&D@1e(@L8**%iI0X%XMgPNfb? zT?Z6?O*ty?M4G46oLoUGk%Qbee-S7FIs(NlaArOI6o%@qSN@xDqYT-45WNvyMx2>O1z@K-L!NiBNj5|K9}>EBZj!83`}9V;kH zAfQbndbmJYeIBiAur60 zWmL0NQZSdpL|aQmpoGAzwJHBrqtw!5cDyj^MP)HpP00=mNiE(q%Ya(?dxL{~j)R?q zxejdqIImT#o*?(W_Z@l1fZJ`hE0<+@-VI~@ZtReh_*d9jwaGZd1P=uqLCz(56+W`p zT~Bn`B=Za!8rsiMO+zQ2f9G)8+5s^z_F9CX_nshP(3#Ds1Ff}eI_8DT!rag)4`Hq$ zLXha?HWzp)nTQ}kh&2W=hTGX)4NQpV4!DGA)4sztciUWzB_2229hxKft zh2-zIS(cp@tiBiV4#tu#NqsiJ<#)r4MROV3q=oUq8iZDUVzM1afZ@P`>Q9{Z&Hh1g zM*iy>vCg=)PV=?t*rW03iPno8xtju7jrU%uZ8qZ`i)K)$e4YOW;3)#wlZxXSnMSbgqqZ2r#y zK^2V~7`?VesW^;*w?JCEKSxDBNZcrGcuX63-BYr^t|-5SdiePtPK)DJ%*~JgeD%Kk zbx5M;4)yzt_9uC+|4u?}0`>bq3Gce@v{8TZzugt4?hZ^b&7iI%%f2W#!C%a1;9crp zSe*0oXe)B6O9;Un*ZS+V(dO{%$uD@HIV(qPi>;s; zfDJ2lJmUJZ`FqY&OJ4_i8Imq(|I!r4HLRiF@8G%6Ss1W-z|G7~!Vqb%`R{i?T&mHU zP}j0uLA9D=@DVH0KUq1OKKoR+r>iA5wnO zB)}+3R>|4079uE;FXd-Zv_PYes80f-9<)~f9d{b-w$}yys|F)e#G_iK3u_8S&ACJ< z9wxCwP&os_Vcn0x#+!cbMOVWbp@`e4=-9%AO|~-;Uab}CNlsmZJ85dMuhpJA7qz5h zx*amjJ%0rb6>5=6_ysBne6qh~am>Rj7lPw9LPSwb9!O z1u_y9E@~#`FIDk@I9swWZFj7nAF02>Z7R~kZ$t0$Ue^2S?(>nXiRx9Rl=AW7F{=MY zO>|P-FcTSq8dC^iKOC2OhA8K@&1Si0@nqDwJTCZTuI={!_By|cFS`hVc{Mis!mJ#- z2e8$f>ho*Uo`CDMRWcQzSd*)W7cJJ%!kJ=*IR?emG&chlEKc#~3X3S>Xe4WR+z9}- zr+*$A|E?-TLI@0@)j4lecyU&W{@O6g&BXK>TI5)VSL=#Z+=cslcg{d+8%%j`sB2Z3 z!=?1IHJ07i@tUf-JPv$CnW>hI?}}C>teHn7)`eBH&|LTwZ)6Ue; zMzuj&BK?xfcDoc?`RC5Bo9Y*ST$k8!*z#GT-)X`~L4D;lal1{=YUz+0)jVF`Nle{!T$co&6e2EIpL*fg^|*wl`I#N%i3U0eOp z^g%f(h7ashIWVu)e?$pth5iu&ay!SCzJp^>55lZEHY-D+M9RIVvF?S)ky1AB) zcZ?N|e|yvV6ov`qGCC{_>Q$r8qf*!Yi>cFZDWhK(k;a?WpC2nX>&N>VmxiliN@}6f zxBBEXu#8WK=5VTxCI*h_z;v>2r^kmtC5y1rCsb4TiW1Qe_D2p>7zAg7TMGXILMOYE6* z`(X+UBq_r^iANY@1T$pe8s4M}_4p6TKHoly0z0OlU<4=|nS0BXsIBBQ`pGppf55@$ z+48j6LXb4UY|JjQnuYjV{zMvKr;XQ{5P0m{XzpnmZSS{de3kjogB$puyh9 zdfgU>02C*2rWKvgCok|s&HBv;w<3Nn3*R8J&4jmPuI2E_aRdicEiTSa-3ESL4LJ6% z(pvUqc70?>QcT~%YuF${fMfgvbB|KdNLw{Q9Tmx1r_=Vv-YCxK`&r~_}<0f$?M zKV97WS7;is4j*R(QKt?-yDUGMGP7Ohv-kG-cX?h`bKdDHMNRnC$CUv{pr zkN@E*Ym3prj)S*s?Gfs$HnZ&pF3#d()8pHxWurZ>jss$Sc}>E>Vd({n^iS4^814x! z&)M9W`FcBf`Z6>&1kDqR}Cqj7S~P+Ruq>P6XEFrlgGm>(&s+V2ZV)r--Ap| zkR#^_Lhil&3|thJ2|f>3F4K+1XA}E!nS;>t4h;?Sruf%>U6tv6ZtaUpz`FS5!|RlU>xlnJ_6XI>dmBE~37;E_;IKZ5nxfp@9`zQ;R&r zS1Y(! zBUzFTtdtdKj>@L%Jo$l%mjIm*!Vr_3<}QKN|%?Mi^m-!NULVsi)9X z^#V7O2s?p-Ok8|nR1QQYVzv=Blx`a4+{xErIeN>>{lDQU2KFn3x8V)?hx1GZTsNoY zpF>RPnsZ`ei*?Oa%!sz1@<+t4p_pwIUxdn6uS#NHy&tX;R$z8{hkt=;Z&o2M&|MC*0 zT+`>i9oI%aGk#p+p=}6hg79pgbq?Bpt!71j_S~g-whV{%Gy3`dhC520?KyMz<%8MH zqP_!KMJpoY59IUF>?U!3KPZoqCjVE!uVOJ6?9dBm(3J@)a=~r)e!vzG8&rDR4;OlR zu@Y~DZC;4l$Xw9nu*G+$-{$25iW)Qt3-RxeDDv`q8uis(*{IAXusq*?1)!(PXm%(3 zfD>kf$zp^oLPb56;So>yVW=z&gDuG%G`r;dIM^;t{Lb1Nqn=9$0vbKs?pJV96!QEm z=o;$-s>!w3fq9P#K^W7%$;NX`fh25*EkS7t)@XhWo7(eil~-!)Hrz6i!uSe%~t#hEf7ffnNVy)5~yB#N4ShRiN!>yj%4YxvbnOqJ5arG3w$(NWjb%b_b^1OFfu9 z#TFz6O$MY7bfAT;gg7W}U9^XJ!IF;YnZsY zgxMa@*ffjhI1fy=Kph)f{1LoUUDQ^@`go-oBV~g3Wt1+ko=|>=H6_%CUI11$p;Ezr z>h?gNPaKPJ85lShCI;1Y^f1C7%BG#L6|t_M%sLN#7JtPNSZ0IX+jT)rEZ4P_>36-w zSr5>tR|h)yln4p=xRg;(1v4?*hn{i#=-AwqW*io>k_Q5hLc-pYVOUy49@J z38AB+VMF@r$q4s=3%E(yL^x3&(XuJFz2PZ1D@i^(#sZdN*W5$xQa_XM&D;->-`+D$YV1hy;8z?cX036~n9fSFxV7^z``8 zcDR;BAng1P6D0??5b7YjzE9pgv0*bo23n3CD86;~Y(yx*eg-*}p9P`9J?w(}ZpSA| z!AfAebXhGOQKZA~H`U7%f7IWBb9!H`x6p1*n`A&rN*$P-Ov?GiNK-Sb9q30IjlrRXEwZ@zs6V+N(y**|d@c3&fuWh_)pa`1i^2z9vD#l(uHAbxm;H0KC z+%dPk3AVS^;c`;#c8!2G-i~A5>QU7=`U&kl+u8TMAl_zT$+kHSOAWlwn~6|*>({Se zpf!qwM2CN5?ZW$Bo_n6Rc^hiq5JuDI23$t$5q{| zc+fxk;>}76x|?h~+8DcigYzRFurJ<-axvxf5rGb3oo8C`-CVh~y@%nw>_Tl_Q|38#xnDVV?RW%#6HN8B5DguT z>0o(n=6|o@JP-BtoDxP5gNlTkei%UqpI1*kSuH2Nq7fUspb`6LxXuJ>3ys9hTy_UC zv~Tk`JDe2X40-S!0Cg0y8A1QK^~B_dYX<MHNTWR)?MKXV)Zo--u@26N*~pBH!RTrSE-#|5p1)#~ zIYH|+*nMc*45t$4zY`=D7pvC`;vU$9Oz%k#{@CU8x>he<4Tr0Y-ymlzBl(sP&V=WA z9^RqwJ&1v3A`uu>{C5!F-ksmQF z^+P?y=Z1cXFX8^y?dBK_cz!@5Mu9N1f(Sm(P_qb%8?^j1w1?p#00`6@6I1mbW8&}< z!$h>jtpU}{X4Ar3Ag5YKEi9wA^>aqsdf9sbza~UB8ccw1f0E1dw+h$)+msutlVAP6 zc6;rCuZKzv(wHthzq6wBOW)o-d+wyqR{Rukyuv4RGuubuwh-JU=H<;LC@{1O%G z{nPA>v>+K+&G0;yzN^LKY01|1 zvF)y^We-!(74BF*9`4E%uiuw@1HtdO6m_=F~JZU@Izt8z?U0 zoP6bit&8WijG2BoZeT0#jCMrBj7Zg!Qr=5vhMn^6d~OMB3i>n|D;jJ* z_27iWR|~fkr%PXsSS0Z#%{VP>D-1EwIQ<3F5j_h~V^IftV-dRpKKy4)-#GV$(#o5b P3_#%N>gTe~DWM4f#0?p? literal 0 HcmV?d00001 diff --git a/rplidar_A2.png b/rplidar_A2.png new file mode 100644 index 0000000000000000000000000000000000000000..3ca929485264e8561bb34473412181827b234997 GIT binary patch literal 47081 zcmXt=WmsIj)5eR#;_hyXyA^kLEiP?wFYfMI+`Yx2#hpb96sJgWcXxeHpa1oKTXwIL zoFtQE=APdWrKTc_3L*l%d-o1iUQSBm-8*RDGn5-5Jn&7F5oGz@JBoMmQsP>kImfvO zzF+5v2J=rl4cZV`e1_9lsD7x6t6|`SslHP%MjEv8ze7NS6<3o=VWFUq#7ybDJ6`oU zOI;TFR#H|P*2Jy6l=t?sQ%1OUms>J@Cve8CRuL#gl_W=lB@ah%=naJx_3y81dYgp3O1m{RPzseQ%K z&cB8kri&?(_Mzr2xGqL}ok)`iwRqru^VRW&-bm9O#FogW; zGQJh>YY=uXgWPY{{?zX%&py)8K=(zWa)m%!8Gh-T&YU!Gym)koierOKnp17Y$-pb6 z{Cl$^$>2>+XGR)a!qzZ$UT!h*Bxv0tE~oppG3`V-n#Z7jt)o3`DWR}RmGsWGZ08r% zCsCSfxYoZqwZ!Yv0dqn7Unh~1q0E>ra=%yu3xk3@<2}5>23l}(gSNzfy*DoWH~zu` zkB)N)GNXkVg@f&U&D@O(jsA8%Qxw(q`i3c#A{R9{MS*~bsAqf{=L`Gsd;H2HS_K*` zB$O1!FP(_?KTLa}$MS@UZ#FE*ggi0)o^A(%U=Sw1sgfJ9$rgRuoViLY#{%J%4c?oX zuy;K%A%>i}3>g$JMb0n!{`&h%W2iDkjwX(DF;hK(-}k<*C7r6!dELg6)uDgj4@sCb zXhA=#ATvAee6=yw(mHYL{YE6iPn6wy3U>^#}4wlT-a(1cdw8v*W#O0TX< z+nSNRgzmyDWje1({tDH`gp`w9X7vSFXdNrIaHZ%X%$RG$;@1J-MYSR%KYKSb{XvOO1p=} z&PZzM+166a=igTsTc!K2;u-$`QR&2!n`s{C?)Y7?FTykG)wy6%4`dw||?eygz<2a&oU5w4FfhjkSdDT^`$xpBLkMu}Bc6 zGdwUBqBDWMG7VQlZqQn3Xwxo4|DYDi>m-+emuxE zA<63fh9(`u`p8}awIMBD>==vLd{cf+Iz#!;><45^Z!F`?_OdZoC8Zqh8uHz)+qz-V z7F-;pG0SzYjJ1MaPi&&0=$~V&Zg>=dqg6yRB{lrq%@w2j%+gc%8YD!XQHTYvv)OnY z8G~r}h!s0lHF+I{Zg#RJKUA4It9vK~W~UF@fLDf5HLKgrE^q1f$hOHn+Rrq_bO-1E zt@AqI-be)w&*VWdV+jHY|BR^Eo~?5qbJnbDZ`kn9pDaU4@}7{*x;v0v@lCjM>%`mw zEa!BS2c!OkK@UJ-L-UmxJq4Y~u}#PDH- zuHa&s9Zot^~3n$MQ(Rx$*aBYFv&gf)M(hvX%7-Z1SP4#IqFC~(PlJVMr*K@ z5tzMJ411SGi*H#C(+GRaTkxW zRxRzPJYyM{o^)! z;Oowr)2YJ81J{GhR=u0QlpP%1m*P_BsLTeJt{2{4P<1O!N zN`JUBh)$A@t0_E_LznCe*B$|d9qRDCOwimUC!=CWqF;mbyqa;tMYgx->Z?eA=Ir+_)OMfR@z`3+|=Leyu7>>H?{-}xXl5YFhvxsXkKP!YTG*p-VDk5 z{bT)zw>o8Y+%qx1&ai}sAiqrrye({HVQVuev)HteVPN>($x!lTSH+)S=ZfYfoK?bg z{xFum0c~69&?J6e$eN36$NQxHsPBayY~k$i?}ankS}9F8B)*`dhRw$kvUAsVIOuD< zwTup-P28gQ2v-{c4w$@f?{pebehFU&*lAdSULo`-ba!lV-Gi z{0|pCH<_+4Pt^8rui1Iu#LfZlhfK3f&UrHMV0HNeaPJ(N?#z=xO$3D4=8MMHnXCrA z%^;h8H0~Ua@lTkLHrAk#(CvhxX7b_xLVyU<_nw@YdCH>U(M0lLTUsTkDptfOduzV7 ztY<`4vOR<8;ylEN-SIX(_}X7j1i*@W$J5x|~oQ8AkxK zjI*(OGVPG}nl6}q0zm-yrE00ji@tbUu@lP2LEN~@R zpeX;Iy1sbpUFzn7=yZl%Ga*#^q1nEvoRi*>U1C#XG(K=`(@R^Ml_o!i|KZZH>d1cJ zW$+tw>ixdg-C8~rOyo*E!IQK8U1EbTT`^iOu;VZ!w6nbkdm7&~oeqTIt@m!Jy#LNuP=BJX8&t}j z?QwFNd16bq=OV&QXGD2z&wtbT!=)y_AC!lfrg}@lb4QmXRy%-A?epgqLYw@aikp(S zeSBb3LTVt7H>%|QLep_jnW`_1zOd}uEAAy0M16X(-UdEj0R1#1KSFXSNKU8)t-d|Y z+N~4orEGku$B*5DCRiKZ|0a#PbfP@&?b7WH#L>$b<@;MD<{+Hkg_&T_KN^1y6*r7& z@OwPNS$(41Au*jycpW!aaAtj#N%}IO-B1oOc4Y9i)Kn11!MO~9!QVd1Yho&Hb!{cP zoOO;(TAZDdm>-IoPjpVF911Z^>HAFZ3ZGVZ)8E!dSu(EX{^$$f3nSqmDP^U|9dfOm zqV?w}Ldx`kuRG%X=^aP`&WQWEj z*99(l2#B&*U;BwZ${J&G(OzOnOxayhZB!?6vmSQT3kf6J^>d%R%uzk7VP~|+1x;e~ zq5k^y%gNn6JR}4L8U|)#d;80bPdEx;FyQTP^#sA}&wNWj(_u)G`y%b{g5yHPT1>%0 z)>H-8HsfUH-6-ME^KK3rinOknuch9YgwNn_>9z0aw=tR+Y3J#X z?G8#z{fW``Xy4#@;G@^_XnQate~p=IH$eCqaaq)4n?;KrD#;IP)$0Jt@Zo3+o&gIn ztar9tGbt-88kfl-k1ybSU7QjN8XCGpE>Vt%FlQjq)0BnP|C*x4_ZFwfKRe+X`Ke%d z0#B2cP@nxrT~J|^KuWZ^9>SL($)P8}pc$*4PbJ1~9Rbf%wXxQArV812s57A`_G9SF zGcSlMx#9eP<}rHQ_qHm$z=QJkKK#=&Rt^1;KcJ5P-h`BQ5|5FrY`mNwvAf+$?6Wyi3qDIU*4odv@%au-h!3Vp z0wS3W899m zpZjc5b06Hb9A@b>K0eSqMy!5<(kD(oQ6fm!{V5dw7{63mXSW#eNnSCX758pBd-B{= zq9&{yQ|aSe$TlxQ<_E~wFG&I)I*uRp8ViTbW>)4P{M3!6ZrgV#aZ09tE9jMSN6>!# zyf_Oj!xqIJf@AgSTCyk=(1o15TK2mI>vjZRk0KN>vsV31pr4Ywey!EWD{_97-9I~H zbHNP`38{BK(GLv`MMQ_`MA{mRo4L89X&hU~6IpmV^~-osvo&Iq)=(W*j1@Z{`4G9% zDiw*4fXUY9xQtMw5HyF;RPEC@ws=&wWtZ(C+2d*wQxn(AWQ^U4G8csO)eFf?;yW;d z6`>M0@#(Xo*DBzJY&pI(qDI|~s|nGe!8qc931GzR_062pT)M>E^{GEaXqxE-H?e}# zxV($#x73~wqwMS)l1Gb2W+#7DZeAY+ zLRB?{ug8nk@&<{#)wukhJ)I|9+a@3_6oA{wR0_UIcm1xES;Hs9mo8nk{7{ zTUL_`fztoY2G;)$dMu}N%4U#pf%^n3EkP}D78;BqoFe^GH~5+kkY&>EUqsWDLQw=@ zjy@Yn=RsL=MD8ju;)Hn(tHy^Xwer|Ow;^7b+RRIJ3_&^xlN0QvXaBHCNrw{M8l96q zkui7b$4t$B5dSt}__c`I<;pU0;he+H;09y9;o62)Us90>-CLJT9;NAZUUhk)2QduY zVRpmjOG0m4K#KQ$W%4XEE$$hX{J^9pM!QdWMsp8ZEE*|7@7`3iqfP0UP)JE}wzO5x zMh#Tyrx+Xzj`lPvn;D^t1&f84imf#US7ukT8n!H0Y+?j%KX0fi-z7>>29Uae2reZ8 zLQ?=42kK`PK6+UNA`x#koXE?<-|zLK{3*gD`sn#ZkkgB&H%>lA#rtcMFH>7D;Ou>> zjD5yZU+fV;Q(2LP*lycGEYwIBkZa zu=glFWb$Wg(gd}mt^o}Owt-~}dJgQ~Y62A-1N4;Lj_>Ok=UE{>GMgyA2ARdV~0hHp6 zzC`n-umGo%N~B@5ZnfzLr$={*^R?q?rgqjLX*0UfY6z92 zkN`hbZntDP?kUAWHL`yXe-R>Oo~$h8%4+{`1jJ?U1C4MjJlG_MFLl*2GGVhn8);Jo zg`_RbUnroMuJ+>t!3(m5k8VlMVq8XUOj16H_Rvw{K9~2RtavyXO58*zHXS=x zIsTH73qsvr^Vudg%%2vRI1d6&$J_+36aFt8zQqSrq1f|f~ZihO-hq#-e^ec_QmgafPv5VL2Yyeg zP);SwdR+TQD^}VKs!d{Ga;MqI)mCIFL^-f-NlzmHwW?Vt+@iQswlh4qfq;f|i9$y6 zXMV8aM1-xMU-DxZU_KU-)JAOo%*Q6KYSJ}w^$`v$!7ex|4qGU^OO4a&*Z4#HL~c4k zP)7?!Dh85x&0!aX08v=xFDmgLa&C@QZF%=btE)O-OmBZePA%7Z}Nr8Rnt4Uv~O1}H)m zJ1;8bt6=}N{MyAE_9~qGmLy=o^+71Yazyy}RMSpEV8Y)k_D-rtOGig+Q=|;vJSfdy z3^|$>%RcaViax*3IrTr6X&%UGzd((RH9g%dem{>m_0}`{i+ESrw_tb_Z+?t{mHD9V zI~E9#L4hC_RdVZ4H`%#5>;P$#3%LEhV^NZwlijpgL9tjH?w6{8MK$zwfZpE+almQ34uj~{0y)ZArt;fo@i<+#VN9lvj^YnF>2@5Kk z0=n0;{6u#LwlTw(P%E$+p_g%XT#gO&K$v2_ta?UL_>AX*jFXx{S~|Qr@oPjvN5!o54y>M%9&$W}^6}$NQNjM8 zKtIvXq+fdu_2Ek;>|Aip0WX0CpJvQVO<~)4{ieKk^{2=_5UbIY7%eB*t|>`DM7t@D z=kKH>X{lWmOY$CJ!U%uWSN2OvY=jS14(xYRlY$zdqs<*3N(@XSM0TxFy zC4OP%h~ur;KC+otS|f{oFyI+#u*o%4`=MiV*>&M!N2sHn8*7C=R>rp3o#5(O?Vb>v zh~Gp!ns(gN`uz(*BU#ipa`ITRS9fp#=#$y|&GEC|$D4o+Ej1L46y?c{4SbHB!uEB0 z&0E!EDke}m`fL16fT3jg9YF`*5gC7l33FtRC{tJ8nI#0naS{}gN05N<#1r9q>vjC( z#p5rq6^vP{G*nz&-Ecjyq9>zIV&23ivHge5)cg}dM1089^T1jE{b&0EEgZ^&{m-E_ zIV=@e86iEZ2#3~R%kf|FCfd<5-f_{_>bKW4njm1xAQ_9mD4+Ua!H0XdPNY}`d0%9w zNfy#z*{g^4D+X9R5U)}BLsaiS%_ZDdOo(^z-1v|rjP?H}z66#Vw|BNtkj|GKnd@7H zRInLG^w&Gn-N+C8YbQKoCPFYQXdMxmuDV-y1rV1ZOX|llwhT^BPX9)4X!2^qs0Ycf z7gt==7mt5N5hvcxas|@=Y+PASm|r%lpOf=ppQ)Zb<;$s+ZT3_loR5^>dLH-DdN#kv z`@Zdx_;%egf~o{9&P#O9HEMCH0^GSjpH58wNss8bXP-NLgpsm;K%wJ}w{n@-vf{80 z`y&-x#7@r9o=$1CI0}g|BkeKMAU0iT2=VNsphjvksjYg31f6m%KSXC*5+ps!xBdu1VvNpw$q1v1lv5U8gpof1C z86GC8mQ4tm-*H>OPY#~FR1iOT(mgRob)-3ELjI}Ue(<1Yzz>~p?G*9#WySM2>D@Nz zT1ZF)YjIU3Z*Z8aAd26sk?*61iD!AN?O)k^arA70oYv$(nwxBDZ7Q=>)deGNoGbrV zSZr_azFsB2UUP6!z|fGb%ao;-2e-X>?fW6C2JM`_serpJ#0j&_xOQ?c_!;{9jeKja zdIMyAO2K0x34d===YZPK@?t?DS*4J-ez+oEb%T_OxZpL3;?1`jA#Ms{0-kz7&>HOJb(N#qqAQQ*Oy{l4D)eGWS zGBlP$8*8knKMc$|6!hH$57KTpTDo5`6Z#XcB3*qE?fXM*B?%tCND*I6II%L59#jOx ztE3T~)%J+ohHz&7KMR0^U%Cak)f{&5ffwPW+hoZjHGoh$;!99NFhbDo&rB)_NqoK6 ze=Raf%J}qoSQ^UerNJ!S3qr)DkT~lc2i`vuAH}l%%wZG|wU!pM#@Sn{Xop)D$QV|H&Zu6vo6oa%&_P0e9_*y9U4J2!7wYBfXjG_~`? zHtTc8w(5PyEz4&#dn6O@W6jt8ys!0Oc1P5sNb-yEt8bW=i$s1yO8qmPMg>~C)B<4G zOggeUPM(epyvB%_T!xe>!eO~z>cUMp^Da(zeJg&<=Zi}ra=yD}Ewd1hv@hFyQjTvj z4SY>V`zU8qKR0HTSLDiX83^fk$a#OCg;y%XI>|^m`;RMA1vw2l{_V(KzEHHm?ZgCO z`=0jcQvMCo-O!2sau%=iC(o^cF{;GDYR4KmK-ZRF-Z?m<=sP0ZqQLNI?BE>NH#2oZ zSQu#(jVokzwUR0cs2u2#O_Gvbu$i41h`KR|vpzU82`XAkp|}9LzHnGkdN^OyVlGc| z;r0%W-!r>sa9W8*gfV1-Qs|bbhKR3^Vu;n#w1Y{-@eMb|SU9F+r}A8%7|vzdrGULj z4GFtZjXf-^oFg_Gq@MD&28UXGyuNt`7vd(1VSLY?qkL*trM|qH8oBb4yWceI*?IT< z!)D%B{6=w|1urT4tb~%{Zb~*v5*>e1AQ^=Wr0FQZrE-b0oZhL$b#*IT&3+T3uhfU8 zw7}Y%n}0-P;V?#e_*s^60XqAoIP{_H8poQF8YSPMFUVx1vnU&xwsS z6ufUZ40?LJleqIQW;Zugg_SykjG)fHqo$tAiqnEF#TZa%%utsoRC^KU;tHz&Ji}y6&sf{7^t#EcZCt6R=lAcX=#>t9m1PEc% zX#T9b6-JLI;{{mQbnfR)oDvQi?=NRx@LiIVsvCksI!Q@b97INX7D(b*+6wX67Uq%Y z@mV4G1}YYtFpNp&4)03qsfGNWCzZD8GkR7lN!!|(>h{d54xPCcy!d-**pLVx2KXNN zx2bpxpfm*q7X%E_Ix{e^c^uJ7z|3M}T9|ifhj2-uv41Kj+~UQa9yyBlk$x zsBVt)>(KULCjfP-b+3<|bKar+o0JBo_l`P6ep9a6f!g=^EMcvoN+4L|cUN9t=6=Up zFYC33$y^XeVjdt+lYh|XR3qT!0T}2l$(|lqwJ3B%- zbtGB3k(t?4GmIS%0rxPs#V2ZUwRk=hHhxAkQHKKk&`7l1LH$N#_Z;(M8sp|kBB@0$ z1CRa@0SJCoZK=cyuy~{1YQ&cwy8j7&zGPm+%BO@Lk~G5(k;!ecwL+lK2ZXP_XG0!l z?V~~y$-g+rg#X*4LFT7$`SCCu{a2b*`p@)BXg{U)YXkaxhkx$>oyF9aujb~immcL% zEqV+8t=H~FZs(n~vAoOA?-4-a*VY)?I+9`{_=AwC85ZI)9g(r&iNmz^2t5*7%Te7< znvxV?YAM6Cd^c-!z9qzeBu=nkLGIt>|L!Q@EOrP+!`b~#B&X>(<|1Jd&J6J z#h!ib11(~)7M58od(*X5QG|sEM~KAU=o%&&@ig$=>aKSp=6o<<_!&li#o{^$WW-h^ zWJH=}CVg)*tob}&weyl@F*RE6%{51x{3s#gKnXxgYFbrQTfJ&@b@d)$-F}>CXPu!r zn>2=qE|BkwnohO(xJn~`OCfSOZ+GnRp-UDEu9EA0dnmDHyNhJMaq!R29=>eT8ut$@ zqY(%=bBBL%3S{7P{>$b+XhMT65{p5cvA*7uJ~}Te2ETnlYjJFMFpT2!9+s4S9vcyZz@0zNKL{bJdgcVZ z1}X(!gVxytz%ESu0O8m-)ucfmq`~r zV%u+jGo^}bUxmnH*5YLgWnJloR%**;UQZM4Qq9nGA;od%$BcdxW>v5In!fe(=xbyK zk(VMOpIm@6j9zbhE3pC>oXfcRi)Bpx=?|tnLm_DHQrwv0deML|KJf* zcPVpZ5EV*1nSr}&Ni8fHw0vx6r6Wqj4_8oHCa0FK{k9UrNVD z+N3!ok2Tx*Lt?M5Lx&Yf?Q8H%kJZ01LWNTS&Gfc=4$8no5 zU>+X)w?`iGwSXl_4Tv*ay8WfjqjgP9?NFZ3U;uYb{+aZv+%;S$E@(HLF5|CFhn!uN z4c1l$hXn=(Mqq;ID**yJ8MEJO3|p6&jSV9}id9wbDA0I>CKU#O<347V7+Y9@6qz(r zZ_xW!^L5Qu;<4pgl)2>pwhwJZmCsz*rGrMu8^lj}p4%TaecL$5{J8=KE~n76-!s2l ziif^F;{hs5NUEd3KQ_mAu%o>3lS_uy?3oFr-OXS2rn)f6^g^qbLpA_UNa@6&@eW%J zC!6&p>_FwJn2Ndp-mm9;qi?FwRyT*=SpqS<%kRnje7$#}(Ka1VmoaHJ$I%19e7p>q zV15SQM0M6&=7m%HDYtLpQhvbe%f@%qPB6@0C;SrXSG z;=dy(Ermr?JP2HT^IsM>1KZ`@Sn-@nXr%=<0K=X zhKb+94eH#U6TgX8o2XKiGXPq$4YV|rFi5jZ2{U1DA|E{g$s$K+bPQ2|3Qd~aclL(??OB5bxUQsBL(LSmG04WY!2s|pJ&F!e) z!kge9%KCLOe@$kyx6z!a*!GPc$avZ5c{j~2-6Y35hy&U?5bc8j9;@!MT~Wy^MedqZ zcp|5Qa7?j4&dCWZZ=NOW(-f~uleA1!2Et`ZQJa+*Nz_nhc1Vs^$JqvACT^Nzld#}z z0?+six?Nr?O`86b7tADLQSreYm^#}TeAdoExg?5mK5n@L2vcnb)hWOkM%7ykMf<|h z32+98Q%%V-E5)tb*~8mjMZd}8eWr^#2zsb65LaLZO>Y5WNoY|O4nbB%U-@4cr&AuH zkxis7f^jkE#M@)~gdk zgmp89K*Erv=lZS>W~MrQJop8yzx~XTa-m1DI)f*-RP^M3o(~F?$#;9Xc=DGiR3ND1 zP*ry)igw{t%SkV7`y^S|#@FN$*lNJ#1PazD7c3$x7k(-E2na9X9@_qd8_%$P4YB9Hsj zq_rAU49Fp<{;WEiI(d+>kNu|~Myjn3gbFynV#MH9q=O2WN8Zi^B&jz(sWC?P+eR-# zr>2g*)7X+)BI&~#YiMqpBFoqemf^nW@m}CPg02i?MIC3!!*#(+%EqTSIH$`H@afb1 zRA+>s6j(2lUHDzZxMV1}{?fWK^+=v(N$TtgtQFzBH5j!VN-F4{ zg2YwFWpTCpTY9Fnq=e=`J8_oy606Vou+*I@^k1^5@yRW`z7ZytYkXL1%=MyILH ze(~qW#6$(y$$fg#C9vU9bgQhU^d>kmgFtMWaF)D06as>t&fE5e2LsvY-EOL zR~VrCE%R6=Sbe8SnzbxZ15$qivBk2_yyT0N-?YuC(BP~uBnr+4d!=qqp885B^rk{1 zWAwpnn8QpTV9?dZkPe~fpL#{D&IiU{q z6zbh_k+1egtLKDwBFBw$_Uh$gpU(*a35Em-;ZF|$C}dsxV2LDHIjV$8X{7%LpF@4{ zl9rob2rSLyAgTJ&ZJ{dAi0teWAm+Z{#j%mN0^>I{5wur_KX6o}E9smhw=hGW@wHba zhI@%Q?4xJt!OWHE&Wwuf{6`gNVSEljyeK+_Q^{`-i(#Byc`Uf;W(3K@Dy&?Uf^>ItibE+s3Zr!oiO$;ii!HB- zBB6%^a&do&JV2O8Pk?~bALYO5gH~FZGQ3jJJ=#!8v$u&NwxFa;T5}J-h@1>tD-O5v z?7(n_Kfm0%U|gMcEf zLQ^4yFcg`j369slEI^}+aFmouB2ckW{%Lx88Uq(Mtfxn8N=Jy77mtI3BQhoi+nkb# zNf8kN1_p+Jkg#|1o!qYxSp|1WyD{a-{&7R4uUcT7%{)FyNy&`tY~-Nz%}r=sUEP-U z_P(JZ1Qz`U4ExcCj?51$+LYT;CX&aerICxTsIg8|Fq)MM?d)5tb>hL58g^aOrF&gN?TH z7$hXjp-`O&I56Dj@$vCi(oXp;*wOg(%140TQHeSff%7QQe;cj~vSFGBquXt9ot z>gW)nWai}J3PYd*QliTX@i`M0ZrIq^{DOkWQ$0F6_Dc=oPq!yi4c3|&SvJkH$)_G| z8^z55sMMOSQzXfnKp6X`YKu*{NF5{qO&gcu9kroiXHs3AHdb_za=6@DPb*n!r@w5( zNw>7B%`^{qmZ+Iy;3O?<8(VJ>7-c+9I0hM?HFAi19}f@Dym_ru5&!<-0g1!Z&W_nZ zsHUJm3^8zX(;NpE7wJsWKb`}yn8e)7_y#SQ{((irLNPG+K5bn?1CB(|jnO;dpKkB|JT}g{9S>g@LIr?QterHa?G!m8fdeY#N9cqWqF>9;&SsLp5<=%_;rJU=dtvEK04CU9EC@7Z zg`J;mvNu@vyIa{`ZspQ%vSSF9wNJ(;k3MfC?}7YG1uJt~o&|$;Rc3K+rPtRPz1duf zYkFJn2vX%dahq7$=9hT5H^i!AnIbS6PZX2D2OP(6_oBikvES0uRX;1-xAloz)}Re>#o=OPPs@( zNRX>uU!{!D*=r{a2Y{a=Ig6!%55@m&0+r= z8Lk+$Qjilss69XI<(o&SaERgie^beo(hGWkyGCI*2^`EW3Zx#hj_vI1413Dg263-9 z2Z!fgL8E93I)#%&J9=VyE}z7gQmpG!Q&CCf!FD3zz>a+=02j&?;s+z8KBJ&~4GjxJ z5*M#$x00kum}ZA=$(*Vbz7kEtE6 zed)-RMjV-49mXVfqOOFAuqd}17nc2`LNV{XTn~2VO(~)4Ny*Z}?7zJva5W}W)?@!J z^zQ{wJ2lVEYyN$YH%HPr`9-FHIS}wW6)a@VEU=3Pg*U6_i%5{LmlPnOio!v~iFTsm zkbQZhr=yED60R+NP>qL=3Gb%rlfLL7fdPS(_Zv<3Ls~Hp)!-8>SD^w}9rq997dn0T zSdBZt9i)bW6TL$L&ld=2&X@7z;r;~X_8E0)K3SRfxKxwbK2CCm$G|0V4|U-p&wCZ z-w5XXu)zx8clUa^Fyd3aBB<9k?6YE$avMn;FKP z-ci1A-lRI!`m`tvYgg+b=$ng{`M5}lD76Gy91KahgdZLfJ)KxCv8c4jf3$EHCauE$ z{o}X(cDz@2+v&DXNq-4~`1|2A&^PtX{z7(u$q$>zSRz~;TVoo4WbxRBvR52hew8EQ z0yZBJ?ej#Q%p(A+>a{o(vXd>_|0^N$#Iy>u=5L)Lwf47|w32r^cU&^L1bjmO?y*)WFtNZe% z1VO3DIPhaErvCk9%GpkbvkcDmAdGNAFi}Q8&z2{eaxL7GkQzI?_*XCd=P|>hyS0D> zVV_&Xjf+`L)s0{D`BFbW|8@YCYn_Kt0ptR#pD1ZqdA)hXX51UogD(IhoUyJdt}HiAg;|(|wT-^4J_RKqowd< zarKK9vg=9?cLLW1t79s5e>_p{!LcnXFhjIsTVqeFnrY~X2oFYxg6RgNZ=IxM(E zUA(y6VhGLmVJDqxjNSL%spEb#1|r#GB;c`%E_5EqUWJ2UV|9Jd^bH|+Fsmro82(fv zvsui!XFhXJOmZw&1QjEWZ4^xCZ@O&K`BV2!M@^T{xBZAnWZ4%f;r6^&7rPA+Jw#)x ztVNym*HQT>O{+g0-H%^bXAEewsf56ii^8K2P}C@HwE(rHl+61M-FtaHp|TPuwCh(k zy<%r_6M4b|_G?{!kiH&Ubul0?*7js@ko8wU`NxD_$2>{%AY`*Ut3b@Vx@aS9n3e@)z z0Bl7Z{4OC-xd}_m=YZ4hK!iplLL?QAa@u)kOo@@PFT16@HD>Yput#oY)zV%jURY2d zUIs4U2h6T^VFEtXjW>>G|gmw408dHtNOLd`tQ;+{s!+)bINC*o-6?+kyr1F!8PF%s~xn% z8>tp2%4!_Uu1Y%xTy4nqm#Y%a2S{*Ts4KgG{tfvV*gyA5XR^T`62p|?3*la1$x zD=PV<3lAbs9eyi1hKdMA=LnS`u{%*=m@&-)zThJBqd(O}ut_rML(@Hz6f?tOnl1l}Ky?%m}AdB8I!Kw;xjZHD0+V;FxOVXt?Sn4uTNJjvO*^-XwQ z9?o7OyPA=yddasM%>g+6B^n=*>nudUS-QhIlz?vrO?H^21)-3$DOqw{0Y;p7?Ta)3 zCB!^XPvo|;qR8CnG9Lid|MY#n7@BJLaLf_%(l{*r<*a{um?h*|Jf^9p`R238N z-6I(KHkySL^5tkIZereA@2r9Lvbi0G!*T{L_UOQXHUV-{z;EskGq30@#Ri4_Kv1cR zOdd@F5rX$ty3ddZfezh3Z2JBg9NbD!6<82y{s5kkE*@D1j(3MvwDG`&x0wA1)4yft z`WBT70Jm7p-6pMtN)xV=ulV+pyWf)9Uu8kO>lVuzKk zm##(b<6a^wqD`8W1tMZ@y$r>=F9#ZC%PqwpX`#P9wEX;Nm*wDb%1B6L>YFcUHMrHP z`MFGV06LDU6LYy#lYf%Ef|zY?H?edH50s@w!BJ9h%KeL2nCj35AzaByZt*a)5Cv53 zezFv0!H~RPv*$6$4%^8(_REw%xcxlc#Q%dQi33n%n`YJ3KZ2-|#w@2OcHbr-Cvr&) z$Y<+3Hh9YjB23{d*2p&$wf{)}9JI8sOD=R`UZv2PJCl?DX938;w*Znh@b3yvoZRP_ z6=QpcD%gJEoL)7~Q@|Hk8XhWWPb3+Rx*xt*lRTIxpAdWE`h}|xm%9K5C`h^MejrJv|2Q@w?Rg_*coge&UM8kwRlkP-K1jQe2eBc z#P&G{LvABRX0@nLekr?q0(n{9^4CWKpoH?b7sgVkc@$NI7twP^mAkrb6;Qeq&G9G} zRs)pb&C;PTv471)virjD(;<(;D;kRE!7|TFCX^_nXV@2}ZS}ypKrs(qZN)cI8(k>P zKH#4mANyI`;7uoetM}JJfB@7<#|cN(*P#>i@02J>lf;k`<<*(LV(L;;Jrjc%Fju5{ zaJ`F|O8X`FL44sHv-bH1Zf2{&@6&^ZG(Kw2YGgU7;r!*7t$M=%fY!c+CCpW7%joi0 zK#$gc>ewm758c3ht;wtuM+c6xR-S*iJkB+m@AZ_d1hZ4R5=^Z_XulH-DvZSicktgP zdu&LcgVo@-xeHrf>Y^S~2-#N>FD19Xgu3a#{|Z>}U@)e|IsRACsRlbGDB%bVhtbDP z;*SeNvO-`Stg*B8yZ&>SZWy4v8_~+{q4R=2+|d)Ik}88+ZxQh*wo(?O^#EHP&TQik zN**9~vdvY~v1Fd%ys{GVPN)4JQRm=Z*YigE#IRdH>{QsU0(Q-1o-{-QMS^!c(*L;A?;24n?bO?q- zC=PdK(038cfM!ktb&;UMC2J<#-`61Pjh0@nt;OiycpGi(U=xx%ku$SdcHzE^i8wB8 z>>0KNAy1EIx;+jXrklPziidN@!ZH;I{Q!jcF-B%EMClW3V6?Qt- zQlNDrsx_`>Y)1P~XlzPKPxJI)Kv%zgw2!zJ;v%!oh5~eSwx@f9yQC*-Zz7IuDELH9 z{@~CiaE(N7dU2g72UNlkg#^xpz)M9d9n`1EHe|aM`UiCg7 z5WL-tF&9EH6MU(61ZRK5bI3=?=7l>e>O1ql@LZ{IdcOHrcz+|$-2GJ|>|ohS7Ea;G ztW}oEpN%zni>w>bPHE`3R4NXeXfk=AVftc?UbH}b91(wC9SfoKl>YDM2>qrLe%x;A zPn4F4Cas$c|6>w;BM!KXrMUjTt&D*0w%MIAvY54SL&G6bLVnt+Mz2-L6H0qUBoXxE zpWF_-;E3Fk~SMJCI66 zmpvyJ1O^1A3QN1-3Mi!`fCUD_3iKo}h%)|L>rveJEZf0#@ntGc*ytl_CJi_;qqNuG@Y=1`6T9zOV~C} zM6Gae(#%R4=x&=jurdK4ui{lSPdQpj?aPYx9cjA zDl#jhn87CbmHotQ*3?dySP8N$f=$*hRP!qal>#bp7D<49q-5n~i_dE+D2^v6ry!BV zcJC+5)*JGN;_6PYve3}S`)9J_z^+z{TP9|;%Q!HRpLN2?G}FhGHo!4E*pnYnlQZ-;~AJLs|8yT`qZzfU1;5_5kY*EUj*lnK>@8C=-&aYC=^CWeBX#mOolfr6q( zy%aJ4hTpFw!?2`RgR0}w)&Q+@XJ0gxg(IeY@w8B2Ly^Cb>P0Hjy z8j=oJ1z_+##>L1GZNQO=UVQHtvidIR_4hgrTHifOE=%(BW%6mJ-Dm>4D|5-g8{wl$zqYqe?r(h_Nai=LmG&_rm`WE1p9 zVYK+xGV!Wvp?3Vh28aM6K%&u?Bo85ehk0Rzg%(&A)GfgTz0uoVshPVvE9FL}K~+QG zlEbq`kG9rsFN$6!Dv7NkNZs_9(EkKsaNGm5U)TkC?vapPP0dISumzsBg9|x+wZk(^ z6om(h=PwfklEHmHY2>HaSkK0%h54L^#)N}b!91YY+2V{a6vk0(5NENO7W6!Aq)flL zu{~OBMnERviwH%)*)7ZSM~5h+LDn|>vjsM$PJ6a|6qTD>sg|7;Io}vS=5Q&8o0&NX zoSmFV-UJlRGp#Q7faHeS3CHc>AJy*P-3J#>?kWuBP{XVCBTWrVe$tGHM=7f61~c=u zp_a$jFv`%2i?b&oF)@gCt4(lZWMsG;RtXVqF4}04;CmYisb;Un$h?_+Zk04X*&NpB zd>$9oDT&ju@#u*v*-3# zl?UIf;Da0w1Om~f7%)#utSajYl1~P_z`I!Y`^+Vj*v<7!@QK{vi;eR=x6R^&7axj8 zhQk9#Q6jcl;eo8a7$A!4zX;G#iL7#5Qf>D8V=|=ebI2Z0fY2%F=;XeNGz?Ps=z@Gx z$Y4_|{6F6xcOQqA=z_q>?(diYZ~kDxEh>*Y0W!iAHt5cZOb-C2xGChohvspUU_WN^ zn;8^jcvfVX|H1XQh)g+w{CgvtVN>64w($HV^NO?CxZGj4WHoxgR&Bo=3TCN)O2zPL zJt`BH(O_^gN+GBSAxj;e7Tn57DxR28?p%AN@AuFP6LR{vwsx^ZJ@I#T?BXGc?}r`{ zYuy_AFhK4B&Qp*DB2~!V!JGLOT{}#-XN@xNu#YK>G(F<)u;Mtk36|vZI+}$lnA+r- z6!0S^68I^G@B`IDP>H2{4PeNZP-As_SkiPkG*taJSffJF=D%U9Y^>lD(A>uCcxQdI zP!M-><>%Y|_R{)!C3ig43J<5D^;HE{1vSAL#TDn#%yB)*Z2@-3ib^7GjwigQ(P9kS z#CRDm0m(;AU42L?nO_?pH%`3vl00x(H@$CZ2wjQmWdX91l=E9mMBK`CYF+8XTVJNeQoilQNs%eNgI1@wxPk_j zB3V8%yVU`Hxgdw;&YT6mBVSt2U`0YUYu`l_;rqB35+-a8pWE#HtW6G3)Lu=Nw|Ii& z7X&3%OM!e58NHG8z*c3zSeVomAuCZ&d=CPJ42_KXp1|L-gM}94I-oV)W$$241s?3X@}uVgWRsDAiHPGF&$a zv;FL_z@N`YI4g5-y3y=aldT^6y`qeYQ>RlG7BsJl(cD(>lg`LS7Y$R-#-zBGyaHu9 z!s#iYBZ*x8D?y6qd+??mz>M>ZsfAKb@3oc1&J!vV=0O{m)u(1UuM#ebyJeShM7oTQQ z#8IvWlatZkA=Wbbs-2z0+ULypo}e$cUNVcJGzUgUN7E#kn=!bL*#WuCRK_=4h*ww1 z*Ur~MAo&OQ!-ESV8EjKz?n3#-z~?wB^O!bto(WFBFnf&5=2hXt@4V7sz^#gG@CTM; zW)fj9v{rJcs72#=TU2I=RLUPkeRlvCgqz6XmiG7e56!sywHhplFgtlU^zS*IT$mU7 zyP_`;xyjVUf|D@7nrX10*XQxVeB>*opd-UN6-tk}dy>J`Tb4@%V0L(JnM}2D&0Cd? z#s=FpWSGdB_0I|4eQYN7ngdIBNc3bw2hM-t>sS<)_Dl>cH(JJz##v4XRuL|P!kJFP zpvMGOHq-O&?<&-nA0G`DEVp8VVNiBwOXN+{(Fw3L%c4ybRj_f$ucy8BKi%1)*wBGk z2Ze6;3j$!ETB?*$F*CD`c30e}cYF@Fvc!bJNnzqiHV@jb4$|#S1x~*%Cr$C-X6H7E zeUSVHLt`1*qXzC4vC$l{cADkqsiT?iR1PWP{l!vYKjvj)PeNoLd{hp(k6dWV$gOa> z?UqST&Uw!wT?mdU9bh*h{kh!J%#&!StQ)vPDk^BlC&u$x*d*%<@TwUVOiYr$e%Ay^ zL(+pEshX(My62kz`(pM+_|wgD{ir29FSt!ui_~VUtgzyjc$uix%RIQY_oBlu$79V5 z2sBT`jMJ?U3nfH0qHNZKYnbcP7GdX`J3LSC*NDX_!2T6bu3q584`GnTko%e(@jf^G zW0idGu{G1bqTQXijH@q2(WaVq;n<_^QaPu5G#k2BbptcXDvbIBf+IWo*y-)bS>Sm; ziJZHCa#ATG|9&S+$&NILZK=N!&T4JhFAqfjGZfw1M-EzrQZYEcfbqVgU)zFY!BbO)M?#63OMP%^soROFM;TATYs>9<@-|J=%gx)uI;3HV=@)>GTP{sM+34i9vv(9`papcELRxg> z+R%bTxOGkPe!na8blydG&oYUxnE%WHGnW*QUyq!X1`?wC68b?FjZDklMLXzF)6{vs zLDqAnk@ee{YQVosxSS~9qRUI=eTeS)xza{@t+&@#KWC)VUGtOQ(b#Fp@8L`*ahcm` zTZ9b1t+BYOr>Bn)Qb}?@b~t+zl`DGpy+%UJ&E(&g-Yh^+GwJ&YMn01?U;+>{$Uk>d zZ4r4XblK*TfSfW+T_U^W$j~YdoM{R9Wgsu=j_qgo^2S&Ko~Kw$RLi$W$Y6pkG=7eo z&4mTcket7*Za~+TPfCYCzr9dn)2B|Y-7&AL?vJ0HUCkT*3429Fh1`OJS_K>SUtrCk z6aIKI`*%E7mQuM>WVe$TIXkrkn#j26_>pzx;YL-G%uEn?B%i<{n<9Gk<(zWZ~q%Resi{F5-Ix z(LM*mC=k>ku-JD9O((y7K%8_-;p^IU0{v&ewd-s0pO~xwcv#s4fpw>}u&|KHHj5ON zNc;yTWYyrA|3qKKubzPCF{61*5h^Z2r+%pOsCGRJ?C@Rqw3rHZbM$pNa#NopnYe*J z%bgy~sf>DI6?3I#XQMmiyj@TgB!V%O)e@c(D3bG+B;wn@<&{rysVVN~MiLs|43>F$ z$n7SHBHE6Gc)VZ+*xD0W>}~*Of!W*BwdYy=h=%sRNa|b_dL{TEW4{i+#b8qH0dFvc zk3Bn>TJ(0W+wYX<+AS-oJ7>YbUM)3Yn$B*C4A6a)y^L0I38ac--vtb`Pgei@!82lf zA=Iqk-)T9ZT-L(sZ@jd+YyZ%BLgs&Mil3nq2+x$`Y4aLroRabUjTrpd)@~ye8eAnk zCpA*XBkMd=QJ>B9cZEP%`|4@{jtROYomm)ImWc$+~e#7%1dI)G_i$JFQZL8OYkfRPSY zALAxo=EgsKxF3*a7K(}N?@NbMXSsUN+996fJhcEpy1KcO!?!YuU#%>WTdk&C5jiuR z8LaQ*cPl|gE6qgrb>Q7m)h}ZXfm@0%-w2Yzc0-}T2Q8Q=eo>+&Z*NyydS$2q=O(@t2 zrlTQZfm?!>V$)_WR2uu3A-PIZ3?9_ktSY!pccWf+KyyXKmT>x?T@{6F$0%Ts(&S&G zNB`y^@Ux$tL{+}Jl@?vMAioVfzU!))F8h9inSbpb&rB1K4M#Y>X^I%;7OYg?m2>#* zLwULW5;cxkaP`8h6(?!t=Pbl&eZa@Ra(V&qrRkn4A|Z&l3!*VblhMFa?v}V-4 zm`0AIxE~UHi*NXEEr*{4UAVPq&bV*WX;&x0#Gq5{r9!PrR}T+IWMS4ns;H2sW?9|! z)3(Od!hmzE2KCQL(0@xEbb4fL3v?_)G;MtjVp6uk_;m2q@aH$8$DXoIgo6pLDzWo?PPVovBr%Zky1ZYn?Z;$AS1mt9t|$Lh zkA|e%=1MGA>+NhGAUSfE|5~==ccEK;n9P5h#{Syv2BQ$>?TL~b_2JP4wv^CAjip@E zf0>mFIr($vacXwyjt&->j}kPn-$E%|Q^`sbw_azl^k{zgbIF7ou0_%-^^e_27(o0!-*rwL;b6p2e+tw+ zci-$2$r#;zm3yq43kk$|(SKwJDt%L$5Sj+!;Z!jVyOi|2xw?`2spshK= z>7mP62ZNk#Q?zHdxcXR01FFjACDMWHrbia2kp12=N1`A)C@3g8ccs0dY7ZsWrlAya zYf!qebgQSU>m6LHh0=idl;6y*AbfD1&%Y3x`lPdAseg`os<#Q5a{#BD2)XfRu8O&pfKV#wR&AwGpz< z;bJ#Y*Dzw*bk(#?ZP1^P%s1EKkIPFsyjd01_1!5=Y4n8W314$2f6*1*p1Sqlif!53SiXlwd z1NOIDBa(6fuuD<(nZpiCRGw>eO}KhSeSQ6Ty41V%rUOkDyi?T%OC&iI2dnG@D-Mp& z8UZhUwjOWn=To8ZoFsN%>2Zx{em5#ae$}rHIf&dUB5WKMRUB52k`Mfs#%tUCGH-(< zdULFCTL3uB`^EcG$Nc;OyvAQ6O%wwDH0-1kevcdZGq4aKPra{1K zpz#8%64(Q*^ug?q76G_}c^j_w{4U~@<{GW zyvMAtr&w1nEnG!E3&IM|qGOYOY5e0)eCRH$J;kS>M4QSm=~|x32qNv zJ*XXx1aql=o9TB&aS@3AnXl3bEp06j^wKw+_NyHC{7`Yv=(MH7Lkl2lOSx!1xa^Iq zqYa$$+lG*PTv*D)@G1R*PGcc7HBq$i{RHV_FEH#BItd>f)b7xu~|dP~cZ0UBTjozI2Jr z&s&n+-loml!M9%VgeZ1G`8&I)^jJuT*OEPM5x=$A1^S7B51*|-Y{ejQ_&$-%HU;Fx ztlr%*Su0DoEI~mS1>PQz9Yj)Z^V(=+tv#Nv;dpGg;cWW9+WTJh!+1XJglBz9If5?< z?`Z5))%A3U+hc`HN@0hhS3@U3b3duu*Q9Wmy@xRvl0&i`A@OEa^Ta=JB>1MF_I?yi#Z1AD z0`R4d{aw3P-$v!-{(4dpr2FQl_)}O0H+J@cTzp}RTL$8LNm)!$o;dMni~K4!s7F&tbJYO39u?TqCk;`v=qyocO$qb(BW zl~*)9kFK@%vzI$;F|wL|MTtP9K^BRjiw%2tD3`hcfH;^;lkb@?gX$P$OuziCj^U2( z=;b?MCf)@{@!i*DU4QUs(HfmbsfkJBZyx}qupGTN`u}F2(}rubE&*=xOgegmzizf(T=yKzA+@o#gpbl?nYNXVA8>OfZS5e2|DDDN*Oxw zd5-?JkdssR!{ut3a%YQ~T3+V)bQ24v!F8kMl+f`i$@xZrrG2l@i|c&LFVt1Ez<4 zNisjy<7|Yh*Qz>*THp8Ou+?Tg&*PFxZ8Xccuhi644A1qMKTX)y2^_lufzOw2o_Txm z2bhT`j?Tg1+dDp=uw_p}qi-LY6K$MfN^RigeNXZ#dfCyHF@9F%H6c0O!4Hk_U~|k; z>+d8Uk>z`wQ}VK+I;viCAdqV(X^4HzV%dIzVZz0?ubEleUH z9?@pLX!QpAp;a|PVv!focV&gx_UM<~5jJrBdJkHP5yFXOD=X+xgTwO1XwzVqL=q%Y zh%kFL+t}6>Q$091h(ovE&-K0fB1V8{#j?V>hK_u(c@;qn9WON4yaekx92X>S%yDL* zpQ6=2S#X;A<*<1{5-AiBs<@}YdEiX>d?w))L z)?#CDl!&2r+VO+5e_#<%91T?>iJ{#57+0smqf-xHdTjqD0*1Ru(SVp=O9U)IR6M5Zk9SN93GVd5V*aBmER5l`b=_dqaFaG6{S= zahpPZ@BH0<0uF6PW@5N7YywORxN8n5r~5mz3LJ>PVO7M8u*VB@|og`Q{N z<3x~Nno-0IdSUV8H!BM>1@KufS*e7eyg+(!IHncar~T5ZMTGYFv&YA~O2)N};jC!H zvNgtB*S}btI=6EYi-b#bT6TV!{$rfEYRFMG7UATG5r9|Q9*+5)Y*~I&#X`17E9xpW zG~jj^?7cR`-8j_#*S#CXXSX!j+A#_Hp6s>mPiBaVwzt-0pxBL8^>EPaMiy; z=;mzQr`ue@`2}u{Jm%i`@-H54Ko@}EvcFQ7;=vqqul#}Z_^`k9#fP`**3H9ueXqS{UvQKvguvuCsWPJAOjiv#t{5x7qf)GM$rcUwXqf>ss>$bSDOg%hz@-aE%KFj2_`#02 zv#hG#i2pFN)JLtm`p8G?T7bb#?V!S~IU8 zpvCa?7CO|Y;#K$3N3QCm*|Hc_lXKYYJ+)=-h}7e}{non2P*V3ZEkUz1SG7gH*Wdb? z5y9o!a5Fu9>&@=u(KV!(K+0x{aHw=usk0{w9bw)p-6+i|fO}!elXK%0Yqc>s|K71n z=-|D*lm7KK;h-mL>b`c~)ZErD+rP%^q6r}h8k$RZFyx$mG1nwXCxH0qyf8-Xr&mC( zX4L#!rP&lA{$*$+1bEgtlCY1AEy*_7b8T>9ngi_3$-i}Tva6h;Fe^;vN= zpG}2Jzj8T_>=DmA47s#)wJwi9rf-b6sS_@ocu3Q#ynp=@``+{1(pC2Y>BA9_C#(A!chfW5 zBmXcY4PU9T)y`idvB7>jEM4i-S+vJGg0jRFr}AW>G~*D2YP58~T&vJ|y<|i5lnEiN z7bCNbr*K9B{Cp>|Qq`MzRc;T`oT}T?z6~?F)Dy!Q2%&lI7PioZO*5#rHUFHgL;Tir zGDF2`+=H?>gX`@qTk!8uH(@`BnU1RPcnxT9o6V-R1Kw;a-7$E%jC@;cHAsxy%#Lx) z#l?EuiM_0>_?U3c3wC<_z4OetQy;eC=E+NbpCC##6RrJe+3ei|%QDH560JZvDw-v|Qn_4S0f zUzHsPN-0|-?c(C72d9e+n9CVVTRBC1`0oOq%F{omJ&(ouCx;;ZU5PkILS#4FHd(1> zmTGiw@0f>Yh>ujLfZyUQ)*1v8dAakr(%?b0(b)u^|Qya<-GS=qquLL>DL z5DoEYUxZS~u8jJ>8nnGa4lHeeIdDqPI2uQ*_wOP);;JE6sfbwoQ9T>05&v0FaxyLf z4rTd6O@J=y$F}BqZ6pD;2C>jj=3ecPBUGc}($a#;O10CkQJ@|g#nc zc_UX9?{nbk_v2AXZbF`HCWqdva z{?gB2eG(UamH|^i@sT6Xg6od`xL$^DHBaQ$XWh&zmH( zDrZTphGad*u=>LKfq%V+rt06k*5;v|k(Bx?Z0O61c#vk^_WGLuN4aIXUoi5krVvTj zqg#PWp&v?B^GCL+{3iq4!pqwqh@j-MXwc9F3}gh)@SXF` z_=mr?d&n^GY;yXDm=!gDbAbWN9pQsCLZKcf#tSpad}-iT*L7IM8JX!ucH|otmy&p; zr7Q}N&0S49-&vg%^r-l*?OBwS`4unzv}}W=1C6S7GL{$tUyYwV4Llz2%%!>r(ASHU z{Wt56MR(oLv}mZtnSb*RiY2ew2{|P7b6tWwt0PxG#G1pa3jUyTyR#wV;=0@MS1QrD zxvL;!!*k4T6@~5=si-Zr9k+onDhu{^?7%HJd>TwoAH^CrPi zlusEqhXogG{(KU=emY1qsGQw9nlaP3fOLFBQ#CxvWesrftSJK5Q(7o4f4N;3WxSy9 z56PwTX+()XKa3|;6qx69o2{H%%B8&Z$(%mgD1~!Q$`M39uM>4hMU#Yjeqt2wG&+k5 zCAPZL^*IQ$fk2YJ_2HYx;Lh84T<%_-r=IRK_jj%PjjpThgkrZp>W zJC?N;uN^lqzK(CzJa^8$y4?Ebz4Jotzjw8c zzvPBd)x|tJdnsxNoA9*=xA^2s1_;TWPFV-rAThq)rjtg0=)b;StT0N$b+sPRvQQ8E z@;5#pPEQVY^%l#l{nDStgaTd7=mB-_=H$=_{J_JF6jTX1H)<0*tZ0eX269K9F7=R7T+87 zPQvF3dwhIs!|#(~RNw~5>gV*TK9-oNIp0MRK*G)NdF2oS<5i)m_x)Q-<%9NMNp3Uq zqR3M96x;l&CP%xcAzsaECDydML(cqAbgwS zrqz!l{9{9W4ayk|M;2B2nWw8qLBWoZ#by7ybV83s0|OgH=q^Dn=*&*(K6eAB7b8RW z)yuC(=Zy#&i8uyms;N>YyZ>|Uv=JuS+3?sJ%JJBYJj@H|8R|e0lP?x_B=D9q`d|g? z1sJa(@RD~;ZKU=h=XFT?ekmRY8b??wJURCzuCD(Zs6YyRX?s^gL)Az=`(h1U!-|G= z`;wT(;35nv<%}qai6?)c=lW#6lLm{3sL>8*M`3B})Kq0_|DhnK)PGgZr$|G%rmSks z`Q7l7jw$0TR7F_&UgzB<`?>$J^}9La*@O*s_xt+`d8r;6d<;uXXAJeoN zItJ0+=e~D0n+AUCwX4d~ZF_P2^YT2bvw5vZ;-+^AEJju@+r|n+xaD1>2ZrJ5{aoJW z1zH2D`xYBnn zDB@l}u>S_=!Poh(T*YSPM>3@y_?;Z(LPg;-32?ZdX*ZW+S(@ElZuQF7t{kfv8rFt@ zO^Gsl*~>o$lP8kdxmRAfRC!ZpGsF=|>BpXO?! zSQ}@gi4;i|I2A_ZFp&~_f55zfyaR4urRIPxju`p_FvO3%F6m_=@!)l= zQ)7$U-F=t2Vqu}F?VUSq|tpdR};y&>h09mH;edJB4XpRlMW z3LeAAa<{j))g6?z3YgJ{K@e32vxC30QAJU!qyK9RsnATLPV+cbl6~f+q*%C!1%ijM z!?#_jD=AI>5jO7cS}{v#dhil({w%q=cQAVB;j8UL_;9`);%U3#)faaBQML9IpBp?n zMiLerPVyI>i~K$yKKts$`5nI2l-IcLCvu*&mUtbyBzPK0nPns~3cT$9|7HLsP_gW# zriT8X!9)er3>+Oo=ZgBgA%LKUP>Ek#2M_I~*yQ38Xm>8$X-(78WcJAa2m`PDzVIA5tAAVClG^4bbR3x#>}2F%7a1jU>8SsBW@{`!p+b{ z&-HjN>vyrdH+K5`C>Di1YrGh$H^HwWQf(xBV|?&9e04Ro7rl@oz1okb^VTgo zv$5hn^}s^o#d)$5i^-y`t#BJNwcAUaNy5q&dlZHVLY`x``Lum9zn!E}oe$)7iak^w zXT#|8wx3wB%vy9!lIdUn`s9N^DxrmKQH6}~_@YPl5kM!BjzgaVI-6W}r%#nq9IXY~ zQaB_$yb6l~Fq3T^@1P|ODTJQ9L~*s)5m8@?K<2qXh)s6OVj#B57+i5{6rTqQK?IRu z98g~o9;t#Pm!+s;Z>W>-SCl@OV1wMy00RV8+&CWdq1CSyY&MoWD8GOOB4gpaWA?RTWpj z6Qnpn!x~O+$VL*1zuss)XacMXxXmQz8>VR^nV*oq_F4W1-K>}l&m*CQxg-K zhExY3|4+fjd#Xm2!MO!Qp!tY2TN;*v32c)FB6`k-4InnFK0HwhY7Ex;ypHI079uoQ zcXAs1QT=h}7Bed$rciBvK!q9wu?c}`gec4^AvQw;8+8A_p&9h@{Hmu&zg zGJ0dW;s&J)`f#D6qt7$IE(U@q&(4TYd5+b16nn93k<$kGeGtHJJO=u$%nFAQ!+!8I zh6wZS1;VAV?TMn=LHsAD7VzNmxt8ljCQ1b{`G4R%X@%~q4+8DGBw9YO5V#eTe!uvd&^#gt$jnW>*KD@8O%tpx4+|2rJTYE?R zCfGJVemU>IYN_~JMK@=85MqX6Mua@mX6u+%sq;V;aFdIPg7_HPI81-THh1h)YF26F zikX^{S60RuUG^;k07U38B!W`N$-3ST@4s%OCx~U2f(0;P6t@diT2;2(m$UMyyygt# z%&!DpyHrKyTe{|qeh`2lt@Ns$1LvZ~gY~PCIK4{bQcXc2ra6leRt)1mooG-6qKfTD zsMPQqEn)inu)k8Swdf;D+7> zqb#}%gnerWO|(wke8!(z6AtEQveYmch=OpD0WWkjmg6wQj&2zvCV)8 zU#sWP{;w@6tbeXf^eTp6`-~*6$W~Z%a_X|MVDrTbtIN=?SLiepF7}Q__8n3bLwI!w{2fnSvJ8rZ!(%QKNqO{dGhrn0U3 ztwR_-?k2dm%J-(D0oxW14T_?z#KbFo&{Rj*pyz(gGszFFuB`$ycdjlf{o*JSn3g=| z17Fbeum9d~)V-f7-`OmXC~jPb{JCY#K^;0Z(kol&k9D#j*rE>4VYW;1^a{uXLg@WY zy}9@nK=x<`5ksL(E!_B?oo6ez8F^D#X+I~~DxR&kt9N?}P-(K+4FYQJsQf3mxFqWj ze=iiTD&a6&8HTqJLr>j9-^yu%2J1PhY(T7fx*9}TaNB7gZITfMG5+`Z5F1mjTg-s- zPm$N$9!e==A2x~UI2UrwX9!En_+UJP4jRCIbbrbICW|Jj`r8R1mO`G61gBtnlZCT8 z{j%>KCB1wUjR&}RjtHRMsk2M0QlPrFQAs?l`=}auYfy6H+g4+&nlOP+MHaxMo_}}v z3tUp2%q=y59AvDwoWbYgMj*-~=}FIa>UXi;P6zoP>!P9}oG^Bdaznj9b)Ny!Tbk!R zzN8W_t;{2kvO4(97c>OIV-j7N$U9c&BzhRyJaOq@EolSTSLy?i>=HDnN^TorC+mY& zH*C;;mU0 z5=iHAfb>5L%H}n#m<8&FJ-Y-2^z{z#uHGkah?nv8rGV?fhvP}&0lAz`d8Q=IaMdIp zt!@ms5pG(t=1l*h=hb06gIzjw3D+AvBsT<@p$GYaT^Ajv^HX{LFS3;H_4x@s$bA_y z<#kofS<{S8R0yZ$PVKf->efY=;Xn9{lf>&{LgzUQVhs z_5FcM&FKd7+#t32MUFC@bPs`x*OS9(`1uLwzg729rs)SIQb@Z;0=*x9LFW8smZ3t) zXx+CQFUSKYpW9xSfYetY5qzs>XBp{_c+=YZUL(n$Uey=iLgJez>^2^6yTY}2IK9Qb z6*e;axVFgKde{LQL}1`?%>dk#~3s)G9(s z3@pWcblERlsVZm4k^6@K*9&lR`QLM%Yyr?6r`xR+s<5w5A2N%|61Pc`>5Ugydg(p1m9>OhtLn{`dHXp#Z#8jS=!RoqS)MG?sqxb*SB0+_qeH@l~UQDA6 zG8T=tw~Bf0p?o=1f}-NdY)!h_Ho-o4_H=qLym+qFcoNU zY3w}hU7x(*(W1yjtq_0>S8;IMv!l$~s*R7l8-848yJ1pn>gOEUv)?s!Hw?#jeVjc2 z1#>YwyWSzcCBV@48ll27Ez_8uW-F~E7>9!~ncEV!L?x*m~sT0OrvfbWPr z(w=K8z;;&}bsVS^mN{Y!8f(Gn*?&L|QD(PCpi0YH&bt2jyBfzRzma6@rl=HY?f|aU zzJHV+ZRutq2zHT`i*P#~NO=c&<+tIKc?gRt$YAn(BFA%L>2t>tN-TDyFg}R?B426; zPc0S_WPIy1FUd`3i!kl>w;AQ`>j*m~B+&7&KBsbp)2|AaOgWOJVPg6w?=+WE+d0$; zk1sxavIY_RHJ0^~F>ZBmaB$&uaWr%$-Wo6N2(b;4cSevZXxb21kOjf1IN2cktG$@S zC+Pe+pe^NK2Uf_YR~3(3ZES2n&U;cRwql7R|3Dc%qxh?h^j|MjC@FDp9 zEwJ!1-=q*I{SdOmPCvwO`aKCLbj#E2H9$Iuu5iO1f2i)6P7Ur3#c^=Jj_dh0i6}Vj zBZg+~Kk?M<8VV!Mj$@^{0&b)!_ORxqV3;0-0} zB$e}1$nxr*$H?bigNHrTG%hUe`71;1Z&krw{z+3b$bv&sVyFV?{+T1j*7ZU;J{L?} zG_(S?!!!n+LU^`&cih)Q8t0V;3&zc*=sQ<8hc70E;22;!sl0$yD3mY^g(QyuqD@>P zi1be6zu|{pRNa;f)<*~mP5*e^`ZT@S|3haI=W?sFT*iG1fUwH*^m{VqWZZ~7x54gj z+YfV3XZO_mRB=HPE&z8fs+pYxh_kb6L5d!E{Wr9PK+@C3e0*Q(fX4Uryj4zsg`vN~ zvzs?__{V6x&`bH>HP3%3TJt^%8fHlF!##Rgty?-QB!SLvM@Lpz=r6!_o#NDXT$(j+ zlMg(h!hcVQPF3Ta{5MDeu=ND8g>Xm#Xuhk=}-p`T6zUP7k!SLCEOkRil{TcS|dY_RBE%_hRmmKJ&dH!lt{v)*eDW-;b7X@BQ>_ZOac-m*0PrS%-Hd}`%$yK*Je>j zUE{bYTc9;iGvBv2`=37IHzi9;x)&>*ipg&A6YGV6$yWal;7Evjp(GEd0yFyiWuRPm zNTVvZU}-AnaJ;}j0ma#ZIGW-;XNwW+s_&bofe$$S#jpUjEx+!*K2ZrN(a_W^kP_F{ zPE#=68cm)Mxqmy$-28&!_9cV+-w;3|G4fq-9VJmHD+JOV_;f+hAOkl~uux+jnK3cZ zirN9MeKCE314)4p+}`iAFj^=Z8)q?={E$u~I@#zVUgf(#s++yRA|#x*8N`9>X$7Mk zM}nZ~x<$*xG#r`uUumU!PY~m@d7cjJjC}U-s8d??-X+U)9q0EtC@p6Duu!echYE%Yn$LTQVUzbE1lB~aQC^S=4YpMA zbXF1-`V9)+uRa8a#C-UmWHR+I%wo^KxO=)2IPqH1Za#p2BzOag9_PRt8BgVs#Sb{r zgZS?bknoHF#i8wUdqQT+kbmtNemM(AJG=E{j>hV9X}7t4<)&%0@rM{u!I*81*>CVx z$8J}x*dg}0Ymvg(F9R8*csfJmuH8-%+F_o|7t2pePR@^kcBcJs-20ZCW%br!qD{AX z)ADNZYe#AS+FjdI6RSrj0)y|a$MNUzw9C>@J9~BHSzdplHEdZWd0$W})WL@is#xuJ zJ6~}o zo3NX2+{~Mh&av`pYqSiZGD!d=3bZ<#Cd>nux ztYG`vw6uv?P5H38t6AyqIxEki&Nmw^lap@m$%lp}(enbx+G3K4Cr?6R{r2+`QG?Vj z8ym|Y@%*vL`%8cOC-i*^o8N>p-V{1E`CJ{~`c(4aM#X-*;sQt8iaAqYC^DVKSb5`q zSM<4*+=Jl#zo+}tS82G-SFZ^f24%TDmXRVW@eH_0t)i{7Nl}a=` z{%QU=-72d`^ywV)_TNAKLjTJoCpszdJqX?}t|vTH0X*e)00Cf*O?IADUA5a;Wr~66 zv|<@C45R2-zv_KhW8twk+O^(wQS8wK(2NJy<&H6Mevw)}8&tZEb!3M^xip*WOBZu8_&*P1jaL`GFF+8hI7tf2~abH=;7|=t1Nm=A|kBy-N4nB1%L>8X@(xM|~Z!-SA z;48+8a0j9{7u42r*30gmY&tcWBA~LBUF@7os_RM2IUlk#g|ue|NPbp~#<1z02QGL$ z<&&qC6t*BKgA6`xON>oQn1? z28$u#n6Y}2U`R+FI(YCB75Kncly-Yv)J=$#rl$P=cV)3g z=p88b;RJh1QD^&|dD5B0y#+r2lH)ai44#=h|ND%CUM{e7v+9RBM15Y>4 zb5$Vjx%OgS(3iwN2Hy2@s=kH2NNv-1)Y1m^WaQW6dDg*rI}8a!R<^fsgh=_el)Wgv z*UBY!^-0-j*=;M>&HX5e^1ej9Yiq|v0(_!U)1NXU%gGq9?1?s#v9+b(}8YMsDzpuF{n zi;wRI#9RpchV|Rmj8@^4y)F16R^cLu(GWsi`T#t+1^F#np&5y= zPyJtx1(<&&G za*RG<|I^aI{c9fG%kCzOA5Wl_ZT%`w=Z_RQhN7FZZFDDQgad@==MaJE;ZJ9K=yM3j z?2#aPFmnZDN_ZW9Zz_vSqk)c~u%>OHE<8zi-@6Hf5U2a6dWZv*>-5{_XIJ5}C?t`A z@v0=zW2fySrp|@DpIA*B)%8_Tj}@5Do(C8%-sIQhVyUlHfQAN9cx>u zUC{uNedb-rmvw+BA zZmOc;#zf8)ExQ`M_Uy#fhIlr7%DGw7S0ZdtZwHEI=!eK5* ziIUg40=sbVD$W+tNdIa&yL$BJ*2^EPJ%-9q4m+9_AAj8ygZ}Y4Z*=JA#6)CU8xYIr zRviIZ!1DKjY_h?Bf~Lv&6)i@;u=BjekuwndH#5M8`b*G|If$Bu0=Q})#@(1@8-^)f ze6nD4*2CcDp%a`#fbeB2%=ezjx#-C*Tmb1H*T}A1z}Fpr2M9GD`|b-Hh-sF!Z}|2( z6-UEyMEee%Yhg_7!xKM{BvT@d(bCd>smGXb8|UBsQa_bz1AN$`L5jfT0(Y1q5anmQ zIJOxC%(|xkK%n(jzu5Gb%zDw~rCaj*IF-X@K}^E*Rl|VE%Y`ITC3)FRm3yI>*jOdN zug&@0sBP{N*w74-=Lgs1_k3>wNnwo*V2UjQJ<9WLedxSj0DWbSvHot+cxgq&QGMcy zk@bLThrVyN5vfVWCmtKJx_>u-FV^5gv15MM?PEsYgl)uT*!^7Gmwz@GH|CutkGDJb zj`nL09p4)>t)K1PH+KT91X}_}4lO%bJA~Vn|6SECN7C)ghd$5USS8#A=-+oP+Pd1! z2isRK2incgC(Tth1qhAY7_@bMZ&>>KuihiE;VM1Z-pX#_Jn&nk2wZMtADk%t1^ba$ zlaTCshP7(6%i9PG{=Hwn)bV@QPv80!?DmX|C_Y|ZUuy>*TK5{hKhMSR#r>kArl(gL zLunI1jt>+)?G1NBMDL>NFkqG>n;3L_W~AVRgoHFj27tp_A(Qja!H~;9pg=g+;uQ9M z0)X?Nh$^`(G4-s;xUMC(!5|lGpME4P)!xjIUPNzC^= zz(+VL^Dy9L~#1pkr?sUf~ofp-Y=EPc4uAPFN-WsrYIZ<7E}vM#OUbs zmi1SJ9hh9JT;rp@lO}0-;)Dp71Wt?8fgJNenfcPlw0o@T>2m9M0l{k6;M64 z{6nqdgNB5pikty(u~``iw(;z2L+0M#mBRA}|NDD}^bHv$DqW1L?8-AB(iF2@IT2b~srYr95}WpxsHCTNx*=TFdEy$V z!*w~4BuM>m;qdduIg~#^m6sEL)C7+O56ZB|86dzJX?vGpD_wioLZe{Cv6KWqKD}^SE$B4|WB|^l-(NeeY@o62VFeQhU{f8!a#?S%hw{I3X&<-zn7QMAb zyqKdpNm%hmC4%A$VGkC99kQw)8VQ$NzsPHuTXJFa&@XwlKpjKLoh$-<#xq3Hne(Td)cT5FP77D7v+~ZF4GEwjT$WWHs-QmJx+< zD+=U3nYM_+E%&rddhU%+zi+XlW=b<;kJ!n&gIaRz5W(+hB5Cp{ZCd2p+{jI3|5!?BQyK$zcZNbA`i z9?W>W)wM8&wR-DBh7wHxR_z;boWd~FwG0w;?hv4GK7CPCWI#3J?Kh@z7 zC9Tg4M@1o_?X3~6RsN37#?b86H7OTD(wNA|wWVcy+oO-}mk21eZb2yvJP%U?OF*oF z9%t2{7Tp(f_s`D7py_Zm(Lxhvu&Rowg%bP|7i)pVWI&T-esQ-cSDv;Y@?n|;e zE`G%D&*}|w$Ea-otAMYbgS74@PtS>QG>^D;`cvlD9L?D^diPN}uG*TWb+n>r-AA0J zO_2yR;=K1_NZ?ozzU)WZnK{zxcfL5Cx^0Mc1J`{@t}>op6kLCqlP=rV9xLyJBe0vP zw}BYe2FESrFAR?J#-5X*ubJqTX9Ruc?H^P+4BZ$#fHfs~Spr;BHXhUC6HUKH4)V=S zYUfkSWI@ZWQQjP#V9}wJfYqqZ_YjzN!;Gd}S@E@qJnRwuh~oG72XasAG&@l3Y{z4<5Rz(6gg1c4o6di%S8`ZNiLF-(5%LM^0?A-a@$)T z?Dsn_#(gvbNg|{1-I6*=p>5^K4PQZW-xMEwSqQu!rsYQ0 zNF&YUAnc6^JN(n?37|QlL<*UGxn(Q(?@6P9FR(uS>Zb=3wZ0{;XQ`3&xOgL@;feuFo(|V0s zr8xHP@@EVQYyzHEesf;sY!@m3aDxP`L{(935k{4N2l8EEYV%3E8j&ajXT>sOtXO2x z4$5}=)A^1<6d)bI9e=NR}as%mP%2M4ymFj2bkzobnv z$fEVU0{1;`eOoqrn2LJ_`wEwq#Bx8Trz4w~m{=|VN`cf*6l|tYKm{YCDgPs2TBYut zp2qF(@3*}N6&J=&ISgC~@bFZC_oa*n+)-~9(hDxXIz`m7a$`R0wG$gVj5puZ4n?6~ z)F}vBFOQ6jXjKB9CsY?#S8N=dJRw-FYc!gp-*vLlp-+$Z`8hdIxw*M~E?ZHH5lf3p zOXU-ei}mu+VW2Kn_8-&D{YMRJTu!|<_H4(q>-8t6!|=@tdK{#FD67{Mx-~FB^F&rt zQ?u?<@l6n5+F@ev4(kVwE*%bteR70Q9)vn@9!Mqe@yPMeUo1zk7np;DiV%*`XTNV{ z^y^Bh~#=rz+!T4avd3ZD`Xm0rih!~fYmWkPagGSKEuU@|odq>U9t@Sug zjEQDGISn)jC#gS+gPp|masN{eP26J1eVMDwLYUn8UUo$G28weaaxnF%oe>SKg4R{L3fFRjCF*jKzz+4c&A|p%X4ar5@;eWYWxnCA5 z;%}M$gYP*#p6$C}YO`PK5IbJ^o@!`GWPwrfpTooIr`+_ki)PQCaQxb6FxxoGO-sIO zoK7uOTMXfB?e3Pt$yixgU3vT9jJ_P@Jq2)Ntl$ih|8`I`QSZx9Rf>4R6`?2Aq)TTJ zk>{utTLI6Gp7j@J_veJNy7fQ72(q3jpFfx0KxlPEgO`@RYJDh>4dzD&cRo0Nv|!(# zvsFIt$E+;Wv%>O@6X$D(xLN1p)pDCkP^0_$`3Zq208vy_)bi-AGmF{+F`52Vrs+(; z*w~ParF(n^6uu|l1fb;cu9Y0HI-9#ONDDsj=BauprPheJ4qGfO*n~%37(;NvT^9twjXXa^9>0x3MAu-1CFg zo2mq>_s*n#!pmnr-8t9y=UfkwoCtoDsO5hI(po*Ch<%6M&_Ax&%Wila7sHyHn|B0| zAKE(5$G5;C@}vYkFC?kl?ole^Jx}%dPHxsqShY^-HkRm(uY^KW167s-@9^O6eB8_x z+YmfGJ+;i(1qAf9>@3)eh>q7Had5FgNyzriDI-`uYTa6>O&pi@$$UT&*QMyBt|FQFr($8h66tp%PPF|k&ao*iHX}=&3j#KDDk?Q2s_b*h~s824KI1%wd8a6C07r2K=Mv;Hx&~^Eq`Td$Z@X$TzXXXVd|K`b_P298m z*!#$FbhoYOcR8zYG&a9ztPkDx(V6dA<>LH7tEReYymt;>Wj&a;DQ|KMZK4F5NY@9> z`|28a72pC@j#7#sPw2CdvX^E|d;S=_9{U{5%3IOB)OFFlQJB5?$bC)3wyQ1lwDpE) zlQcfo*f6Y$-~souj6j~xrr+}OFA=!4OLFZGYr!kAr`_Etj^|TtCN$*ZP8_Kla{7!s z4mX4G6wG~|RV#7jS-zMmnTLr%&Ym0Dr*FDZ=M%(D+8FsTo1r{AJ5ZK9DY|zs9r~mC zvNsE#ozL@ax1JsXn{U1e#XT(%>bAM!26&y|prJcSOX_h1JgvX+Ywiu&XLalbOzmfz zoSc(q{cC3nNka8)F@fHU*8%r1MLNFfOzFfQQ*VIFR#qGSuN zNDs*%xfLvbAXZH>fV~fO-27x95?Wl|!Du}_xgKq%gD#O`2a87U%h%G}%ev4mRf&yW z|7}^@1i6MD-j1L>VSe=y!@$*cQne~NTt86i!5Eho-5v64^&$>EEV*#(4eM;e=Lj+& zKi7ua85TRo4y{C>g&yF!9>%fVFPkqB-(E zcy&wvrZ149lj4Fua%z=0D`0%Fqzg)GOp%#ai_OJ-{(}#5k&&zL>cf!=(Nqnm-9XP2 z#tC*V(yMkT-_9%mpQb+MUOu~IkfU&>5Ha#&C=zt&`E#f8XNtnsEU(+;B3MyL1f`5* za=i)}UJw^8LdKbn8~CaqsbIFmk8=E4t(a#jE;lrk=N+rG5B(twS9eMJbS|Mv#7$4) z`Rq>LPn+6}R6QHMq^wZ=@#1RECFyc52i1UjBCV?|B&0+c%7ahe+Tle{nK$6Pj<-Xx z2{90PIuleH2}5M77HXkR-~5h-i*zQMUn7-n(=p#D`&HyWm(OU2`6sdQ6R@7LO46@m zn3z-zb)%9PVKZU!*xdB-%H}!~;E|PB@Q+_5o-BoC+IH(^#wO|edPRm&DQI9f3?bIo zuZW~9{_b1t$L$Oq?={`zY43tSr!KzMatuU%e)vFvv7NC_`#Vlsnl)y~U?m4xDKv@~ z7*I4)e3V7&dDZxcjEExaK-h!way2V!>p2X1b72q>Hr!Y{tzeM|Vi3kN|EOJy>=Av( zv&yH5SQTqL@YNH+wC_=aolgko6O{-OsV7<1Esr?o-gx!=wVH|1^aB6aDOHpB0|D5$ zA{lpfr`1zuk7qQ$z*AK(*@W&RUr}k}Y|5O7S&a6{-PU=w=ZAc+`(FV_*Dk@Q6}vS@ zs_b#l`RDg-Cj;N}F*YAta?g+x#ocuI{*C&F;b@o=ffa~V0(!p1U8+Iv05>;Eh~64*tmuf zPnSiYt5*34+^^6Xo^wF*lGn^iU@X;o_2Gnfiy(C~Bb|nwmcH3l-?Bp5Qk*17;ep-S z6w8pmj*=apTbv!~>LRH8{R7Ex?I$hGNQ6YTA8;+*7A4|(${ZN|i8EW4jXQSv_Pq?l2pf6~wG@ zAzkiamxgOvtK8~U*q%>RXP)zooZ(a~8Q~~sB|k^Rd=yZADI9MQl0=t?Od*}Sht`!M z;KToxQyG!+5XUE3hQ%V!B@<=D5wCm`nNTz=WU>7@r((Dy;>&~W(MtOUA$h#&ygO-QN3+l$ZfqvQpeT!Xykx6F^L4{eV)Nb*&YbD+~8WqIwr3F zm#Sb3Hel)z`_KgJUPm~%Tbk}VS87%FE18PlsUbR@t6|wYYXxbM1dHz<|6PH~+u+_* zevREcixMU&J4mWL)V`yWi6hEQ`wWBuxlh_oMH-R{KoW_&S}E5_pY62!#Ow6?95U&# zau*I&mG+W!4AD)Kq=A3ykagHl&qn&lfJ2Nx$V@6V3!I64}3d`Vi!G|B$(Q0?dpk-Ff)RYn=gl4W0B->&G zKqAnM1Vr*ZD3j(`AaoLfzw`dR@J%aJCylJP%xIrTB@enfN7SLx!PeBC%syKe336`UwFwu~L1M zOo^e7lG!5uGmU?=z~|cj^Eov1fVXeaK(FGb&z~bde#8nxK+LUd#i<-mIJ>$Ct*cAU zfEQQk1xle>>NM~&&dxOrUs;hPSS7*yxY1l9ZSCw(1NDJzikZ0?%2pK$FYo5O&PI}P z2lC;*?poxE8Y}{Wh!?sYHh3oPqT=HG*4D+vVk+8+(6>hMfrUwwNY*s?GXl@P>hS&N~3wDH{l(<^I3AghL2F@+CDI z5t~@5<&FS`Vamu#A8JI51QBXN5M$5yDn?6-x2-`0g|0?06=FaZb?EbZEm^QHPI5HD zKtGf4vSFrxxsC+p!0#b(x=rypIzEib+3t_f9dK`r3y{`vrgL-6hRfHjA+ja1Z{ESK ztp7O($;hz%u-0j`%~9<`bAfw(yi`0aKq2g-8O{bQYHO&n-l3VNIz}z(9QDFHt_+I` z4`+X?kQh_C_DT6X{oP1ms^^cSI0rDg_jVN zIh}UZ*Ov;|9grwhOa)(>JiG#()jerPW(8^GaDq*xj!!X*a|(HLsIQR*)e{u#@$n28 zrIgH98}EIOM0bZaM^6SUJY_E)e^&qg4cem88 z-O?V-KIyzT*9>HkF(NO`g8g1>Dy}04N~VJ*k2$+y*0*LoA@G#|5INU|urAEIBet>R zDtd`7 zWv>3Q$6K2089w_1&1Ww8ZK^#K&c;Ch$bDgsdJw=kO-`wJubULfJ^-`CY?= zuW?J(YJd;Uda0GTxWqlLzMWBO#;<8A_l6K3Y$^FDLKZ_Opwar*^Zg84m=8Qa747I7 z7(901lMDN7;SyXm?}Pr(yQ`GsoF#rw508ITDCm+Ugnz)2iG0y*bt^Sd&bI0Fp-?pf zpc+@Zxe8}8GBO8H>(4hfdA7Pf`n{PzAu7Uhr6wi-=nyylE!@+{CNzVJZrlh@nGJw^ zJY9h_5O<|~Eoeh;B@^^$uB>EvX>6O>zCKwwWHISH0_RPqe=$Ch{RUFcqjHWLR8=y+ z4jLh!h|vWObrOPoFfWbyQ4Z9s?d&cjQCk0r3u9zdg-IL1l)}kyQ!>gzhegofq3xj{ zZ8KOeNU(Q0EEMEOCMVHeA=**b4hW19O+*GE+Z(#zLAA@rcm2hdieCVj9lxrO2%MBa zHuC|r=a$NV$!a5iGw(+swvuwHB1~R~jN%*#@h}%0JX*+}Zl_;Mw&&&mx{2}%9Qo-~ zx}iFosnub_f=3G@*^kk$yj5kvNidu7q`V0RnDX3u-Fju7lm*6LoA%{>K4U?k@N>w6 zQ<-%BAs8k`&H2J(_zZ;lVEOp^KC*>6v=@lw1fE^ZsOlEgQ1Q>pewHd@hDRm=ElZ@c z_h)~%6}#RTVFy>0lCJEkZw z5p}O4{OWeGEr24;#nKgz4KjBzxTOumi5{gGVfv-vyj zDu#GLQhy!iUnQ$c3oR~Cf7}7HuRvb=JIyqBZZmIrcX=E~ru|=fWAiQ>Pqtq(F7=uN zoxROgex<3AxsP7e;n=$$C0`pYAZ^-8HE1U;^ zXpOwEUpnth5r~k#8n<=f2!>G=Z^d_AJ}wtUvOT&=Z#nxMc~WQc^xOYwkX2(uJ;Xwt z(8Z7-bE(C(cnN5?>lyXhLh9QSzSF}K4qsJA$RJzWK2%KoDqk(8n=fS-0Dv$Pne9?i z!SA_p@1i5Mvq4*0A&cG8FP{2}Mm=U7nt}nt`YgJI(kup*tXHdz^lu2~OztU8vgyfK zNkh~gxtGnei62)_LAUe|XKkZC1Rmp?;Z&x+=#Kt`V7lZ*AAtZyCWrP|wTa|}{Dt%S zRmfvP2{z_UsqSxm?XJL$zaH(IK9Pg$uOVYFFz}SHB!>(wX+;S0TrHh*THd>QlEx<= zLrEUkawatd`^QJQ2dW}urf2rU|dQE&p;Mx)jVir9?Zc_se9;pV-_i@z(d`f>!1Rd(+FYq(;&9C+*2KMq5MQ z#7uWoB@g@D*&Z63L(023C|jSeb&{wGNv(C}0!ypF#*{QsIj=CY)jD{7)g(0UU~c{3 z$we;YwNBdYxNvNp!d*`n#TdWnTr9xV-vmyHUisj_lgl>3^kXU6om-Qj$_}( z(&g10TN+iUvsG0>Z@(Q|&>QPfy2Qc5^WkE8{HJ|q1hqnqz1}8YMlRy4;vpktjZ9lc z#gcH=fWn%hddsOqZ7F(r2}9{JW#y|4EDgR?-+t+am$zXpIn=lEcb@lV_ZZM)#uQ_Q zQtPAGVM=+oxqW`?#y=CJ@064_St79iLHM$90(uI)o%TpR`uFcna^EcaQbK8~hr$S{ zUum!1L-PvEO%L__M@pL(*yZw9sYm+cNaw0o)0%l{0QF6 zTlqmgZ%b@;Y}`SsxaqiauJbho<=ZX1W5orUJo8tlfe-hDH?1F5T4%c=?%$t&KWTj1 z{Knsyu{$a;I5qdG*AMadD-L&G?KBRZ{Vk= z3`#A5%f=jdd$#caxHJC(rTM?7f`Wn<;RwvnKq12RNcp|Hn;Yb>!Z(z_1&#URg9DP9 ziqt}#sfq9b=N|&$5o;=WwSC+6-<%j_w!^j$cbi4}e~c_$Z~nuB$=tXJmzh@CPWfDK zSR=4_K4~!ssgSMlSNz9P|GmC7qu^9j{rBAy! zi2MHQtJjpt#2&AP4N?G`l~In&Ls(e&1n4L9c_Z5{Y$mi5YoPS>=2f+{lm1ot%sepX zXlwho$$joP2RcU`2`>mm-Mn&8ZmrdQgqxSQn59@bxV5+I8B;w?tI>xjC0E3C=k1yS zG*qfs0=NkUmb!dzDt!S1fAi<>&#l*Qqh0C_piS$M@Svc?7*Fz{prF)0>cqlhGnKufoa@T?~h4TUGLASeIzRTI2UI&ZC zJhQEgP;4;5C}IeEzkbRmueMu>K+TW@!$3jFN$0#g9~%M%6{HD=0^nu%!7v}8L+{!T zNr_xwC6^)iVvXR05wmp^PQCfJ!*!!Z`dJp{Cjl6jMfb3Zh?9wTm_jJ1(*J!Oa6QueB!KxSZFmLV$WtZzej-+6{@2X9 z$U1To6h|02xIP11k10-kMejQV{Z6VHDp8=~#B!i4f-2QdC`RuExnRW7z<|(&q84^Z;b%2JgQrXl|Wg6H6Y literal 0 HcmV?d00001 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; +} +