From 68305b38480dcf03f35af319f55ed376c35d7cb6 Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Thu, 7 May 2026 20:01:57 +0000 Subject: [PATCH] Use upstream lidar code --- README.md | 9 +- docker-compose.yml | 2 - lidar/Dockerfile | 8 +- lidar/LICENSE | 6 +- lidar/README.md | 76 +- lidar/sllidar_ros2/CMakeLists.txt | 81 - lidar/sllidar_ros2/LICENSE | 24 - lidar/sllidar_ros2/README.md | 182 -- .../sllidar_ros2/launch/sllidar_a1_launch.py | 84 - .../launch/sllidar_a2m12_launch .py | 71 - .../launch/sllidar_a2m7_launch.py | 71 - .../launch/sllidar_a2m8_launch.py | 71 - .../sllidar_ros2/launch/sllidar_a3_launch.py | 72 - .../sllidar_ros2/launch/sllidar_c1_launch.py | 71 - .../sllidar_ros2/launch/sllidar_s1_launch.py | 64 - .../launch/sllidar_s1_tcp_launch.py | 66 - .../sllidar_ros2/launch/sllidar_s2_launch.py | 71 - .../sllidar_ros2/launch/sllidar_s2e_launch.py | 73 - .../sllidar_ros2/launch/sllidar_s3_launch.py | 71 - .../sllidar_ros2/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 - lidar/sllidar_ros2/package.xml | 18 - lidar/sllidar_ros2/rplidar_A1.png | Bin 33994 -> 0 bytes lidar/sllidar_ros2/rplidar_A2.png | Bin 47081 -> 0 bytes lidar/sllidar_ros2/rviz/sllidar_ros2.rviz | 159 -- .../sllidar_ros2/scripts/create_udev_rules.sh | 13 - .../sllidar_ros2/scripts/delete_udev_rules.sh | 11 - lidar/sllidar_ros2/scripts/rplidar.rules | 4 - lidar/sllidar_ros2/sdk/Makefile | 77 - lidar/sllidar_ros2/sdk/include/rplidar.h | 45 - lidar/sllidar_ros2/sdk/include/rplidar_cmd.h | 215 --- .../sllidar_ros2/sdk/include/rplidar_driver.h | 247 --- .../sdk/include/rplidar_protocol.h | 61 - lidar/sllidar_ros2/sdk/include/rptypes.h | 116 -- lidar/sllidar_ros2/sdk/include/sl_crc.h | 43 - lidar/sllidar_ros2/sdk/include/sl_lidar.h | 47 - lidar/sllidar_ros2/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 - lidar/sllidar_ros2/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 -- .../sllidar_ros2/sdk/src/arch/linux/timer.cpp | 52 - lidar/sllidar_ros2/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 - .../sllidar_ros2/sdk/src/arch/macOS/timer.cpp | 54 - lidar/sllidar_ros2/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 --------- .../sllidar_ros2/sdk/src/arch/win32/timer.cpp | 72 - lidar/sllidar_ros2/sdk/src/arch/win32/timer.h | 49 - .../sdk/src/arch/win32/winthread.hpp | 144 -- .../dataunpacker/dataunnpacker_commondef.h | 60 - .../src/dataunpacker/dataunnpacker_internal.h | 74 - .../sdk/src/dataunpacker/dataunpacker.cpp | 259 --- .../sdk/src/dataunpacker/dataunpacker.h | 93 - .../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 - lidar/sllidar_ros2/sdk/src/hal/abs_rxtx.h | 88 - lidar/sllidar_ros2/sdk/src/hal/assert.h | 18 - lidar/sllidar_ros2/sdk/src/hal/byteops.h | 94 - lidar/sllidar_ros2/sdk/src/hal/byteorder.h | 112 -- lidar/sllidar_ros2/sdk/src/hal/event.h | 206 -- lidar/sllidar_ros2/sdk/src/hal/locker.h | 205 -- lidar/sllidar_ros2/sdk/src/hal/socket.h | 149 -- lidar/sllidar_ros2/sdk/src/hal/thread.cpp | 48 - lidar/sllidar_ros2/sdk/src/hal/thread.h | 94 - lidar/sllidar_ros2/sdk/src/hal/types.h | 119 -- lidar/sllidar_ros2/sdk/src/hal/util.h | 67 - lidar/sllidar_ros2/sdk/src/hal/waiter.h | 44 - lidar/sllidar_ros2/sdk/src/rplidar_driver.cpp | 199 -- lidar/sllidar_ros2/sdk/src/sdkcommon.h | 51 - .../sdk/src/sl_async_transceiver.cpp | 412 ---- .../sdk/src/sl_async_transceiver.h | 167 -- lidar/sllidar_ros2/sdk/src/sl_crc.cpp | 102 - .../sllidar_ros2/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 -- lidar/sllidar_ros2/sdk/src/sl_tcp_channel.cpp | 124 -- lidar/sllidar_ros2/sdk/src/sl_udp_channel.cpp | 129 -- lidar/sllidar_ros2/src/sllidar_client.cpp | 43 - lidar/sllidar_ros2/src/sllidar_node.cpp | 498 ----- 109 files changed, 25 insertions(+), 16999 deletions(-) delete mode 100644 lidar/sllidar_ros2/CMakeLists.txt delete mode 100644 lidar/sllidar_ros2/LICENSE delete mode 100644 lidar/sllidar_ros2/README.md delete mode 100644 lidar/sllidar_ros2/launch/sllidar_a1_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_a2m12_launch .py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_a2m7_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_a2m8_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_a3_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_c1_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_s1_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_s1_tcp_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_s2_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_s2e_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_s3_launch.py delete mode 100644 lidar/sllidar_ros2/launch/sllidar_t1_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_a1_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_a2m12_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_a2m7_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_a2m8_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_a3_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_c1_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_s1_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_s1_tcp_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_s2_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_s2e_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_s3_launch.py delete mode 100644 lidar/sllidar_ros2/launch/view_sllidar_t1_launch.py delete mode 100644 lidar/sllidar_ros2/package.xml delete mode 100644 lidar/sllidar_ros2/rplidar_A1.png delete mode 100644 lidar/sllidar_ros2/rplidar_A2.png delete mode 100644 lidar/sllidar_ros2/rviz/sllidar_ros2.rviz delete mode 100644 lidar/sllidar_ros2/scripts/create_udev_rules.sh delete mode 100644 lidar/sllidar_ros2/scripts/delete_udev_rules.sh delete mode 100644 lidar/sllidar_ros2/scripts/rplidar.rules delete mode 100644 lidar/sllidar_ros2/sdk/Makefile delete mode 100644 lidar/sllidar_ros2/sdk/include/rplidar.h delete mode 100644 lidar/sllidar_ros2/sdk/include/rplidar_cmd.h delete mode 100644 lidar/sllidar_ros2/sdk/include/rplidar_driver.h delete mode 100644 lidar/sllidar_ros2/sdk/include/rplidar_protocol.h delete mode 100644 lidar/sllidar_ros2/sdk/include/rptypes.h delete mode 100644 lidar/sllidar_ros2/sdk/include/sl_crc.h delete mode 100644 lidar/sllidar_ros2/sdk/include/sl_lidar.h delete mode 100644 lidar/sllidar_ros2/sdk/include/sl_lidar_cmd.h delete mode 100644 lidar/sllidar_ros2/sdk/include/sl_lidar_driver.h delete mode 100644 lidar/sllidar_ros2/sdk/include/sl_lidar_driver_impl.h delete mode 100644 lidar/sllidar_ros2/sdk/include/sl_lidar_protocol.h delete mode 100644 lidar/sllidar_ros2/sdk/include/sl_types.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/linux/arch_linux.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/linux/net_socket.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/linux/thread.hpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/linux/timer.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/linux/timer.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/macOS/arch_macOS.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/macOS/net_socket.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/macOS/thread.hpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/macOS/timer.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/macOS/timer.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/win32/arch_win32.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/win32/net_socket.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/win32/timer.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/win32/timer.h delete mode 100644 lidar/sllidar_ros2/sdk/src/arch/win32/winthread.hpp delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_commondef.h delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_internal.h delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.h delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/dataupacker_namespace.h delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.h delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.h delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/abs_rxtx.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/assert.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/byteops.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/byteorder.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/event.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/locker.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/socket.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/thread.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/thread.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/types.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/util.h delete mode 100644 lidar/sllidar_ros2/sdk/src/hal/waiter.h delete mode 100644 lidar/sllidar_ros2/sdk/src/rplidar_driver.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/sdkcommon.h delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_async_transceiver.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_async_transceiver.h delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_crc.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_lidar_driver.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.h delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_serial_channel.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_tcp_channel.cpp delete mode 100644 lidar/sllidar_ros2/sdk/src/sl_udp_channel.cpp delete mode 100644 lidar/sllidar_ros2/src/sllidar_client.cpp delete mode 100644 lidar/sllidar_ros2/src/sllidar_node.cpp diff --git a/README.md b/README.md index c1762c8..d389dcc 100644 --- a/README.md +++ b/README.md @@ -73,10 +73,15 @@ docker compose build lidar Pipe images directly to the target over SSH — no intermediate file or registry needed: ```bash -docker save raspbot_v2:latest raspbot_v2_lidar:latest \ - | ssh @raspbot-v2.local docker load +# Upload all images +docker save $(docker compose config --images) | ssh @raspbot-v2.local docker load + +# Upload a single image +docker save raspbot_v2_oled:latest | ssh @raspbot-v2.local docker load ``` +`docker compose config --images` resolves all image names from the compose file, including any environment variable substitutions. + Then copy the compose file and any `.env` to the target: ```bash diff --git a/docker-compose.yml b/docker-compose.yml index ad5e64a..bbc90fb 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -42,8 +42,6 @@ services: - frame_id:=${LIDAR_FRAME_ID:-laser} - serial_baudrate:=115200 - angle_compensate:=true - - scan_qos_depth:=${LIDAR_QOS_DEPTH:-2} - - scan_qos_reliability:=${LIDAR_QOS_RELIABILITY:-best_effort} restart: unless-stopped oled: diff --git a/lidar/Dockerfile b/lidar/Dockerfile index e4e6279..a87b6ef 100644 --- a/lidar/Dockerfile +++ b/lidar/Dockerfile @@ -5,14 +5,18 @@ FROM ros:kilted AS builder SHELL ["/bin/bash", "-c"] +# Pin to a specific branch or tag; override with --build-arg SLLIDAR_REF= +ARG SLLIDAR_REF=main + RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ python3-colcon-common-extensions \ && rm -rf /var/lib/apt/lists/* WORKDIR /ws -# Source is managed as a git subtree — copy directly from the workspace -COPY lidar/sllidar_ros2/ src/sllidar_ros2/ +RUN git clone --depth 1 --branch ${SLLIDAR_REF} \ + https://github.com/Slamtec/sllidar_ros2.git src/sllidar_ros2 RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \ colcon build --packages-select sllidar_ros2 diff --git a/lidar/LICENSE b/lidar/LICENSE index 325f1a5..e1e98a5 100644 --- a/lidar/LICENSE +++ b/lidar/LICENSE @@ -25,7 +25,7 @@ SOFTWARE. Third-party code ---------------- -sllidar_ros2/ - Source: https://github.com/m5p3nc3r/sllidar_ros2 (fork of Slamtec sllidar_ros2) +sllidar_ros2 (cloned at build time) + Source: https://github.com/Slamtec/sllidar_ros2 Copyright: RoboPeak Team / Shanghai Slamtec Co., Ltd. - License: BSD-style — see sllidar_ros2/LICENSE for the full text. + License: BSD-style — see the LICENSE file in that repository. diff --git a/lidar/README.md b/lidar/README.md index 2042365..c8d9e24 100644 --- a/lidar/README.md +++ b/lidar/README.md @@ -1,7 +1,7 @@ # RPLIDAR A1 — LIDAR Container -ROS 2 container for the Slamtec RPLIDAR A1, built on top of a fork of the -official `sllidar_ros2` driver. +ROS 2 container for the Slamtec RPLIDAR A1, using the upstream +[sllidar_ros2](https://github.com/Slamtec/sllidar_ros2) driver cloned at build time. --- @@ -13,7 +13,6 @@ official `sllidar_ros2` driver. │ │ │ serial 115200 baud │ │ angle_compensate = true │ - │ scan_mode = Sensitivity │ │ ▼ │ │ /dev/ttyUSB0 ──────> RPLIDAR A1 │ │ │ @@ -27,89 +26,32 @@ official `sllidar_ros2` driver. ## How the build works -The driver source lives in `lidar/sllidar_ros2/` as a **git subtree** of this -repository. The [Dockerfile](Dockerfile) copies it directly from the -workspace — no network access is required at build time. +The [Dockerfile](Dockerfile) clones the upstream driver from GitHub at build time: ``` -Workspace repo Docker build -────────────────────────── ────────────────────────────────────────── -lidar/sllidar_ros2/ ───> COPY lidar/sllidar_ros2/ src/sllidar_ros2/ -(git subtree of fork) colcon build --packages-select sllidar_ros2 +git clone https://github.com/Slamtec/sllidar_ros2.git +colcon build --packages-select sllidar_ros2 ``` -The upstream fork is maintained at: - - https://github.com/m5p3nc3r/sllidar_ros2 - ---- - -## Development workflow - -### Pulling upstream changes into the workspace +To pin to a specific branch or tag, pass `SLLIDAR_REF` as a build arg: ```bash -git subtree pull --prefix=lidar/sllidar_ros2 sllidar_ros2 main --squash +docker compose build --build-arg SLLIDAR_REF=v2.0.0 lidar ``` -This fetches the latest commits from the fork, squashes them into a single -merge commit, and updates `lidar/sllidar_ros2/` in the workspace. - -### Making local changes - -Edit files inside `lidar/sllidar_ros2/` as normal, then commit them to the -workspace repo: - -```bash -git add lidar/sllidar_ros2/ -git commit -m "lidar: describe the change" -``` - -### Pushing changes back to the fork - -```bash -git subtree push --prefix=lidar/sllidar_ros2 sllidar_ros2 main -``` - -This replays only the commits that touched `lidar/sllidar_ros2/` and pushes -them to the fork's `main` branch. - -### First-time setup on a fresh clone - -The subtree files are committed in the workspace repo, so a `git clone` of -this repo is all that is needed — there is no separate step to initialise the -subtree. To be able to pull or push to the fork later, add the remote: - -```bash -git remote add sllidar_ros2 https://github.com/m5p3nc3r/sllidar_ros2.git -``` - ---- - -## Changes relative to upstream - -| File | Change | -|---|---| -| `src/sllidar_node.cpp` | `/scan` publisher QoS made configurable via `scan_qos_depth` (int) and `scan_qos_reliability` (`best_effort`\|`reliable`) parameters. Defaults to `best_effort`, depth 2 — required for Nav2 / RViz2 compatibility. | -| `launch/sllidar_a1_launch.py` | Exposes `scan_qos_depth` and `scan_qos_reliability` as launch arguments. | - -The upstream Slamtec repository is at: - - https://github.com/slamtec/sllidar_ros2 - --- ## Published topics | Topic | Type | Description | |---|---|---| -| `/scan` | `sensor_msgs/LaserScan` | 360° laser scan, `BEST_EFFORT` QoS, depth 2 | +| `/scan` | `sensor_msgs/LaserScan` | 360° laser scan | ## Device The RPLIDAR A1 connects via USB serial at `/dev/ttyUSB0` (default), 115200 baud. The container receives access to that device via the `devices` mapping in -[docker-compose.yml](../docker-compose.yml). To use a different port, set +[docker-compose.yml](../docker-compose.yml). To use a different port, set `LIDAR_PORT` in your `.env` file: ```bash diff --git a/lidar/sllidar_ros2/CMakeLists.txt b/lidar/sllidar_ros2/CMakeLists.txt deleted file mode 100644 index aa094b1..0000000 --- a/lidar/sllidar_ros2/CMakeLists.txt +++ /dev/null @@ -1,81 +0,0 @@ -################################################################################ -# 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/lidar/sllidar_ros2/LICENSE b/lidar/sllidar_ros2/LICENSE deleted file mode 100644 index 144df83..0000000 --- a/lidar/sllidar_ros2/LICENSE +++ /dev/null @@ -1,24 +0,0 @@ -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/lidar/sllidar_ros2/README.md b/lidar/sllidar_ros2/README.md deleted file mode 100644 index ccb81d5..0000000 --- a/lidar/sllidar_ros2/README.md +++ /dev/null @@ -1,182 +0,0 @@ -# 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/lidar/sllidar_ros2/launch/sllidar_a1_launch.py b/lidar/sllidar_ros2/launch/sllidar_a1_launch.py deleted file mode 100644 index f8c4ed7..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_a1_launch.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/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') - scan_qos_depth = LaunchConfiguration('scan_qos_depth', default='10') - scan_qos_reliability = LaunchConfiguration('scan_qos_reliability', default='reliable') - - 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'), - - DeclareLaunchArgument( - 'scan_qos_depth', - default_value=scan_qos_depth, - description='Queue depth for the /scan publisher'), - - DeclareLaunchArgument( - 'scan_qos_reliability', - default_value=scan_qos_reliability, - description='Reliability policy for the /scan publisher: best_effort or reliable'), - - 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_qos_depth': scan_qos_depth, - 'scan_qos_reliability': scan_qos_reliability}], - output='screen'), - ]) - diff --git a/lidar/sllidar_ros2/launch/sllidar_a2m12_launch .py b/lidar/sllidar_ros2/launch/sllidar_a2m12_launch .py deleted file mode 100644 index f813a73..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_a2m12_launch .py +++ /dev/null @@ -1,71 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_a2m7_launch.py b/lidar/sllidar_ros2/launch/sllidar_a2m7_launch.py deleted file mode 100644 index f813a73..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_a2m7_launch.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_a2m8_launch.py b/lidar/sllidar_ros2/launch/sllidar_a2m8_launch.py deleted file mode 100644 index b1ad70f..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_a2m8_launch.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_a3_launch.py b/lidar/sllidar_ros2/launch/sllidar_a3_launch.py deleted file mode 100644 index 5125750..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_a3_launch.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_c1_launch.py b/lidar/sllidar_ros2/launch/sllidar_c1_launch.py deleted file mode 100644 index 90b01af..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_c1_launch.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_s1_launch.py b/lidar/sllidar_ros2/launch/sllidar_s1_launch.py deleted file mode 100644 index b11e7da..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_s1_launch.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_s1_tcp_launch.py b/lidar/sllidar_ros2/launch/sllidar_s1_tcp_launch.py deleted file mode 100644 index f882dc5..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_s1_tcp_launch.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_s2_launch.py b/lidar/sllidar_ros2/launch/sllidar_s2_launch.py deleted file mode 100644 index 5cc7b85..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_s2_launch.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_s2e_launch.py b/lidar/sllidar_ros2/launch/sllidar_s2e_launch.py deleted file mode 100644 index 7da57c8..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_s2e_launch.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_s3_launch.py b/lidar/sllidar_ros2/launch/sllidar_s3_launch.py deleted file mode 100644 index a2eebcd..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_s3_launch.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/sllidar_t1_launch.py b/lidar/sllidar_ros2/launch/sllidar_t1_launch.py deleted file mode 100644 index e2baccc..0000000 --- a/lidar/sllidar_ros2/launch/sllidar_t1_launch.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_a1_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_a1_launch.py deleted file mode 100644 index 0959e7b..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_a1_launch.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_a2m12_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_a2m12_launch.py deleted file mode 100644 index 40b9260..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_a2m12_launch.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_a2m7_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_a2m7_launch.py deleted file mode 100644 index 40b9260..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_a2m7_launch.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_a2m8_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_a2m8_launch.py deleted file mode 100644 index 0959e7b..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_a2m8_launch.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_a3_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_a3_launch.py deleted file mode 100644 index 3949ec7..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_a3_launch.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_c1_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_c1_launch.py deleted file mode 100644 index cbba3bc..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_c1_launch.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_s1_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_s1_launch.py deleted file mode 100644 index 70f2fa9..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_s1_launch.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_s1_tcp_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_s1_tcp_launch.py deleted file mode 100644 index 1714da4..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_s1_tcp_launch.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_s2_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_s2_launch.py deleted file mode 100644 index 016852c..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_s2_launch.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_s2e_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_s2e_launch.py deleted file mode 100644 index 00a2b33..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_s2e_launch.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_s3_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_s3_launch.py deleted file mode 100644 index a2a15fe..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_s3_launch.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/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/lidar/sllidar_ros2/launch/view_sllidar_t1_launch.py b/lidar/sllidar_ros2/launch/view_sllidar_t1_launch.py deleted file mode 100644 index 72c89ba..0000000 --- a/lidar/sllidar_ros2/launch/view_sllidar_t1_launch.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/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/lidar/sllidar_ros2/package.xml b/lidar/sllidar_ros2/package.xml deleted file mode 100644 index c90ef4d..0000000 --- a/lidar/sllidar_ros2/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - 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/lidar/sllidar_ros2/rplidar_A1.png b/lidar/sllidar_ros2/rplidar_A1.png deleted file mode 100644 index 994def4d839d055383a94729b95a3bb63862b26c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 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? diff --git a/lidar/sllidar_ros2/rplidar_A2.png b/lidar/sllidar_ros2/rplidar_A2.png deleted file mode 100644 index 3ca929485264e8561bb34473412181827b234997..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 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 diff --git a/lidar/sllidar_ros2/rviz/sllidar_ros2.rviz b/lidar/sllidar_ros2/rviz/sllidar_ros2.rviz deleted file mode 100644 index 51f93ea..0000000 --- a/lidar/sllidar_ros2/rviz/sllidar_ros2.rviz +++ /dev/null @@ -1,159 +0,0 @@ -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/lidar/sllidar_ros2/scripts/create_udev_rules.sh b/lidar/sllidar_ros2/scripts/create_udev_rules.sh deleted file mode 100644 index b1645d0..0000000 --- a/lidar/sllidar_ros2/scripts/create_udev_rules.sh +++ /dev/null @@ -1,13 +0,0 @@ -#!/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/lidar/sllidar_ros2/scripts/delete_udev_rules.sh b/lidar/sllidar_ros2/scripts/delete_udev_rules.sh deleted file mode 100644 index 05c77f8..0000000 --- a/lidar/sllidar_ros2/scripts/delete_udev_rules.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/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/lidar/sllidar_ros2/scripts/rplidar.rules b/lidar/sllidar_ros2/scripts/rplidar.rules deleted file mode 100644 index d75ed81..0000000 --- a/lidar/sllidar_ros2/scripts/rplidar.rules +++ /dev/null @@ -1,4 +0,0 @@ -# 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/lidar/sllidar_ros2/sdk/Makefile b/lidar/sllidar_ros2/sdk/Makefile deleted file mode 100644 index e1724b8..0000000 --- a/lidar/sllidar_ros2/sdk/Makefile +++ /dev/null @@ -1,77 +0,0 @@ -#/* -# * 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/lidar/sllidar_ros2/sdk/include/rplidar.h b/lidar/sllidar_ros2/sdk/include/rplidar.h deleted file mode 100644 index c4f4752..0000000 --- a/lidar/sllidar_ros2/sdk/include/rplidar.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/include/rplidar_cmd.h b/lidar/sllidar_ros2/sdk/include/rplidar_cmd.h deleted file mode 100644 index 6bfc7e7..0000000 --- a/lidar/sllidar_ros2/sdk/include/rplidar_cmd.h +++ /dev/null @@ -1,215 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/include/rplidar_driver.h b/lidar/sllidar_ros2/sdk/include/rplidar_driver.h deleted file mode 100644 index bd2fc82..0000000 --- a/lidar/sllidar_ros2/sdk/include/rplidar_driver.h +++ /dev/null @@ -1,247 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/include/rplidar_protocol.h b/lidar/sllidar_ros2/sdk/include/rplidar_protocol.h deleted file mode 100644 index cddb9c8..0000000 --- a/lidar/sllidar_ros2/sdk/include/rplidar_protocol.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/include/rptypes.h b/lidar/sllidar_ros2/sdk/include/rptypes.h deleted file mode 100644 index 34d8ecb..0000000 --- a/lidar/sllidar_ros2/sdk/include/rptypes.h +++ /dev/null @@ -1,116 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/include/sl_crc.h b/lidar/sllidar_ros2/sdk/include/sl_crc.h deleted file mode 100644 index 184a8df..0000000 --- a/lidar/sllidar_ros2/sdk/include/sl_crc.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -* 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/lidar/sllidar_ros2/sdk/include/sl_lidar.h b/lidar/sllidar_ros2/sdk/include/sl_lidar.h deleted file mode 100644 index db15fe7..0000000 --- a/lidar/sllidar_ros2/sdk/include/sl_lidar.h +++ /dev/null @@ -1,47 +0,0 @@ -/* -* 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/lidar/sllidar_ros2/sdk/include/sl_lidar_cmd.h b/lidar/sllidar_ros2/sdk/include/sl_lidar_cmd.h deleted file mode 100644 index bd3f8d2..0000000 --- a/lidar/sllidar_ros2/sdk/include/sl_lidar_cmd.h +++ /dev/null @@ -1,388 +0,0 @@ -/* -* 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/lidar/sllidar_ros2/sdk/include/sl_lidar_driver.h b/lidar/sllidar_ros2/sdk/include/sl_lidar_driver.h deleted file mode 100644 index 9dade3c..0000000 --- a/lidar/sllidar_ros2/sdk/include/sl_lidar_driver.h +++ /dev/null @@ -1,569 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/include/sl_lidar_driver_impl.h b/lidar/sllidar_ros2/sdk/include/sl_lidar_driver_impl.h deleted file mode 100644 index 3c1b814..0000000 --- a/lidar/sllidar_ros2/sdk/include/sl_lidar_driver_impl.h +++ /dev/null @@ -1,157 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/include/sl_lidar_protocol.h b/lidar/sllidar_ros2/sdk/include/sl_lidar_protocol.h deleted file mode 100644 index a9a10c1..0000000 --- a/lidar/sllidar_ros2/sdk/include/sl_lidar_protocol.h +++ /dev/null @@ -1,85 +0,0 @@ -/* -* 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/lidar/sllidar_ros2/sdk/include/sl_types.h b/lidar/sllidar_ros2/sdk/include/sl_types.h deleted file mode 100644 index 96a3755..0000000 --- a/lidar/sllidar_ros2/sdk/include/sl_types.h +++ /dev/null @@ -1,83 +0,0 @@ -/* -* 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/lidar/sllidar_ros2/sdk/src/arch/linux/arch_linux.h b/lidar/sllidar_ros2/sdk/src/arch/linux/arch_linux.h deleted file mode 100644 index ce31343..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/linux/arch_linux.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.cpp b/lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.cpp deleted file mode 100644 index 2bec556..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.cpp +++ /dev/null @@ -1,475 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.h b/lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.h deleted file mode 100644 index 627369a..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/linux/net_serial.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/linux/net_socket.cpp b/lidar/sllidar_ros2/sdk/src/arch/linux/net_socket.cpp deleted file mode 100644 index 99eb2dc..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/linux/net_socket.cpp +++ /dev/null @@ -1,893 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/linux/thread.hpp b/lidar/sllidar_ros2/sdk/src/arch/linux/thread.hpp deleted file mode 100644 index 6a9107b..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/linux/thread.hpp +++ /dev/null @@ -1,185 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/linux/timer.cpp b/lidar/sllidar_ros2/sdk/src/arch/linux/timer.cpp deleted file mode 100644 index a727e46..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/linux/timer.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/linux/timer.h b/lidar/sllidar_ros2/sdk/src/arch/linux/timer.h deleted file mode 100644 index 1641001..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/linux/timer.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/macOS/arch_macOS.h b/lidar/sllidar_ros2/sdk/src/arch/macOS/arch_macOS.h deleted file mode 100644 index 950ab97..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/macOS/arch_macOS.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.cpp b/lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.cpp deleted file mode 100644 index d1859fa..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.cpp +++ /dev/null @@ -1,346 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.h b/lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.h deleted file mode 100644 index 41db813..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/macOS/net_serial.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/macOS/net_socket.cpp b/lidar/sllidar_ros2/sdk/src/arch/macOS/net_socket.cpp deleted file mode 100644 index 2208fac..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/macOS/net_socket.cpp +++ /dev/null @@ -1,899 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/macOS/thread.hpp b/lidar/sllidar_ros2/sdk/src/arch/macOS/thread.hpp deleted file mode 100644 index 72fd8f8..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/macOS/thread.hpp +++ /dev/null @@ -1,79 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/macOS/timer.cpp b/lidar/sllidar_ros2/sdk/src/arch/macOS/timer.cpp deleted file mode 100644 index cae6e57..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/macOS/timer.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/macOS/timer.h b/lidar/sllidar_ros2/sdk/src/arch/macOS/timer.h deleted file mode 100644 index 8f5ef04..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/macOS/timer.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/win32/arch_win32.h b/lidar/sllidar_ros2/sdk/src/arch/win32/arch_win32.h deleted file mode 100644 index 3ae6655..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/win32/arch_win32.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.cpp b/lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.cpp deleted file mode 100644 index f1fe025..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.cpp +++ /dev/null @@ -1,367 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.h b/lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.h deleted file mode 100644 index d43137e..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/win32/net_serial.h +++ /dev/null @@ -1,86 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/win32/net_socket.cpp b/lidar/sllidar_ros2/sdk/src/arch/win32/net_socket.cpp deleted file mode 100644 index 611c9d1..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/win32/net_socket.cpp +++ /dev/null @@ -1,945 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/win32/timer.cpp b/lidar/sllidar_ros2/sdk/src/arch/win32/timer.cpp deleted file mode 100644 index d153cc4..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/win32/timer.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/win32/timer.h b/lidar/sllidar_ros2/sdk/src/arch/win32/timer.h deleted file mode 100644 index 27b7a76..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/win32/timer.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/arch/win32/winthread.hpp b/lidar/sllidar_ros2/sdk/src/arch/win32/winthread.hpp deleted file mode 100644 index 590794d..0000000 --- a/lidar/sllidar_ros2/sdk/src/arch/win32/winthread.hpp +++ /dev/null @@ -1,144 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_commondef.h b/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_commondef.h deleted file mode 100644 index 70b9c99..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_commondef.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_internal.h b/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_internal.h deleted file mode 100644 index 6b1b6dd..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunnpacker_internal.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.cpp b/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.cpp deleted file mode 100644 index 74680c0..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.cpp +++ /dev/null @@ -1,259 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.h b/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.h deleted file mode 100644 index 2cdf6ca..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/dataunpacker.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/dataupacker_namespace.h b/lidar/sllidar_ros2/sdk/src/dataunpacker/dataupacker_namespace.h deleted file mode 100644 index 9e27192..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/dataupacker_namespace.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - - -#define BEGIN_DATAUNPACKER_NS() namespace sl{ namespace internal{ -#define END_DATAUNPACKER_NS() }} \ No newline at end of file diff --git a/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.cpp b/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.cpp deleted file mode 100644 index ab3110d..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.cpp +++ /dev/null @@ -1,1054 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.h b/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.h deleted file mode 100644 index 008f544..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_capsules.h +++ /dev/null @@ -1,149 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp b/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp deleted file mode 100644 index 8ed450e..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp +++ /dev/null @@ -1,192 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.h b/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.h deleted file mode 100644 index 178a8c6..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_hqnode.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp b/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp deleted file mode 100644 index b799b77..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.h b/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.h deleted file mode 100644 index 9068b1b..0000000 --- a/lidar/sllidar_ros2/sdk/src/dataunpacker/unpacker/handler_normalnode.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/abs_rxtx.h b/lidar/sllidar_ros2/sdk/src/hal/abs_rxtx.h deleted file mode 100644 index 54900e3..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/abs_rxtx.h +++ /dev/null @@ -1,88 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/assert.h b/lidar/sllidar_ros2/sdk/src/hal/assert.h deleted file mode 100644 index 37cfdd5..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/assert.h +++ /dev/null @@ -1,18 +0,0 @@ -#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/lidar/sllidar_ros2/sdk/src/hal/byteops.h b/lidar/sllidar_ros2/sdk/src/hal/byteops.h deleted file mode 100644 index c86bde3..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/byteops.h +++ /dev/null @@ -1,94 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/byteorder.h b/lidar/sllidar_ros2/sdk/src/hal/byteorder.h deleted file mode 100644 index d06a9af..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/byteorder.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/event.h b/lidar/sllidar_ros2/sdk/src/hal/event.h deleted file mode 100644 index ada15a5..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/event.h +++ /dev/null @@ -1,206 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/locker.h b/lidar/sllidar_ros2/sdk/src/hal/locker.h deleted file mode 100644 index 2a18e6b..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/locker.h +++ /dev/null @@ -1,205 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/socket.h b/lidar/sllidar_ros2/sdk/src/hal/socket.h deleted file mode 100644 index cf4fab0..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/socket.h +++ /dev/null @@ -1,149 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/thread.cpp b/lidar/sllidar_ros2/sdk/src/hal/thread.cpp deleted file mode 100644 index bc68dd5..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/thread.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/thread.h b/lidar/sllidar_ros2/sdk/src/hal/thread.h deleted file mode 100644 index 74b6ff7..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/thread.h +++ /dev/null @@ -1,94 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/types.h b/lidar/sllidar_ros2/sdk/src/hal/types.h deleted file mode 100644 index 6e23faa..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/types.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/util.h b/lidar/sllidar_ros2/sdk/src/hal/util.h deleted file mode 100644 index 205a858..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/util.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/hal/waiter.h b/lidar/sllidar_ros2/sdk/src/hal/waiter.h deleted file mode 100644 index 7f83b9b..0000000 --- a/lidar/sllidar_ros2/sdk/src/hal/waiter.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/rplidar_driver.cpp b/lidar/sllidar_ros2/sdk/src/rplidar_driver.cpp deleted file mode 100644 index beedd39..0000000 --- a/lidar/sllidar_ros2/sdk/src/rplidar_driver.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sdkcommon.h b/lidar/sllidar_ros2/sdk/src/sdkcommon.h deleted file mode 100644 index f928b42..0000000 --- a/lidar/sllidar_ros2/sdk/src/sdkcommon.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_async_transceiver.cpp b/lidar/sllidar_ros2/sdk/src/sl_async_transceiver.cpp deleted file mode 100644 index c8033a5..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_async_transceiver.cpp +++ /dev/null @@ -1,412 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_async_transceiver.h b/lidar/sllidar_ros2/sdk/src/sl_async_transceiver.h deleted file mode 100644 index 75774e8..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_async_transceiver.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_crc.cpp b/lidar/sllidar_ros2/sdk/src/sl_crc.cpp deleted file mode 100644 index 49d7844..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_crc.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_lidar_driver.cpp b/lidar/sllidar_ros2/sdk/src/sl_lidar_driver.cpp deleted file mode 100644 index 4fd719c..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_lidar_driver.cpp +++ /dev/null @@ -1,1702 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.cpp b/lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.cpp deleted file mode 100644 index a629000..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.h b/lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.h deleted file mode 100644 index 1cc62f7..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_lidarprotocol_codec.h +++ /dev/null @@ -1,88 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_serial_channel.cpp b/lidar/sllidar_ros2/sdk/src/sl_serial_channel.cpp deleted file mode 100644 index f7faf3c..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_serial_channel.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_tcp_channel.cpp b/lidar/sllidar_ros2/sdk/src/sl_tcp_channel.cpp deleted file mode 100644 index 8351c41..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_tcp_channel.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/sdk/src/sl_udp_channel.cpp b/lidar/sllidar_ros2/sdk/src/sl_udp_channel.cpp deleted file mode 100644 index df0808d..0000000 --- a/lidar/sllidar_ros2/sdk/src/sl_udp_channel.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/src/sllidar_client.cpp b/lidar/sllidar_ros2/src/sllidar_client.cpp deleted file mode 100644 index 6441939..0000000 --- a/lidar/sllidar_ros2/src/sllidar_client.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* - * 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/lidar/sllidar_ros2/src/sllidar_node.cpp b/lidar/sllidar_ros2/src/sllidar_node.cpp deleted file mode 100644 index 10fbae2..0000000 --- a/lidar/sllidar_ros2/src/sllidar_node.cpp +++ /dev/null @@ -1,498 +0,0 @@ -/* - * 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") - { - // QoS parameters must be declared here because the publisher is created - // before init_param() is called in work_loop(). - this->declare_parameter("scan_qos_depth", 2); - this->declare_parameter("scan_qos_reliability", "best_effort"); - - int qos_depth; - std::string qos_reliability; - this->get_parameter_or("scan_qos_depth", qos_depth, 2); - this->get_parameter_or("scan_qos_reliability", qos_reliability, "best_effort"); - - auto qos = rclcpp::QoS(rclcpp::KeepLast(qos_depth)); - if (qos_reliability == "best_effort") { - qos.best_effort(); - } else { - qos.reliable(); - } - - scan_pub = this->create_publisher("scan", qos); - } - - 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; -} -