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 994def4..0000000 Binary files a/lidar/sllidar_ros2/rplidar_A1.png and /dev/null differ diff --git a/lidar/sllidar_ros2/rplidar_A2.png b/lidar/sllidar_ros2/rplidar_A2.png deleted file mode 100644 index 3ca9294..0000000 Binary files a/lidar/sllidar_ros2/rplidar_A2.png and /dev/null differ 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; -} -