Use upstream lidar code
This commit is contained in:
@@ -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 <user>@raspbot-v2.local docker load
|
||||
# Upload all images
|
||||
docker save $(docker compose config --images) | ssh <user>@raspbot-v2.local docker load
|
||||
|
||||
# Upload a single image
|
||||
docker save raspbot_v2_oled:latest | ssh <user>@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
|
||||
|
||||
@@ -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:
|
||||
|
||||
+6
-2
@@ -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=<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
|
||||
|
||||
+3
-3
@@ -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.
|
||||
|
||||
+9
-67
@@ -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
|
||||
|
||||
@@ -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()
|
||||
@@ -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.
|
||||
@@ -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: <http://wiki.ros.org/rplidar>
|
||||
|
||||
SLAMTEC LIDAR HomePage: <http://www.slamtec.com/en/Lidar>
|
||||
|
||||
SLAMTEC LIDAR SDK: <https://github.com/Slamtec/rplidar_sdk>
|
||||
|
||||
SLAMTEC LIDAR Tutorial: <https://github.com/robopeak/rplidar_ros/wiki>
|
||||
|
||||
## 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/<rosdistro>/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 <your_own_ros2_ws>/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
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -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'),
|
||||
])
|
||||
|
||||
@@ -1,18 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>sllidar_ros2</name>
|
||||
<version>1.0.1</version>
|
||||
<description>
|
||||
The rplidar ros2 package, support rplidar A2/A1 and A3/S1
|
||||
</description>
|
||||
<maintainer email="ros@slamtec.com">Slamtec ROS Maintainer</maintainer>
|
||||
<license>BSD</license>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 33 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 46 KiB |
@@ -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: <Fixed 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: <Fixed 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
|
||||
@@ -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 "
|
||||
@@ -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"
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
@@ -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 <vector>
|
||||
#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
|
||||
@@ -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
|
||||
@@ -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<RplidarScanMode>& 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;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
}}}
|
||||
@@ -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
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
#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 * );
|
||||
@@ -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);
|
||||
}}
|
||||
@@ -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))
|
||||
@@ -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
|
||||
@@ -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 <vector>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#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 <string>
|
||||
|
||||
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 <typename T>
|
||||
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<IChannel*> createSerialPortChannel(const std::string& device, int baudrate);
|
||||
|
||||
/**
|
||||
* Create a TCP channel
|
||||
* \param ip IP address of the device
|
||||
* \param port TCP port
|
||||
*/
|
||||
Result<IChannel*> createTcpChannel(const std::string& ip, int port);
|
||||
|
||||
/**
|
||||
* Create a UDP channel
|
||||
* \param ip IP address of the device
|
||||
* \param port UDP port
|
||||
*/
|
||||
Result<IChannel*> 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<LidarScanMode>& 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<ISerialChannel*> 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<ILidarDriver*> createLidarDriver();
|
||||
}
|
||||
@@ -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<LidarScanMode>& 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<sl_u8> &outputBuf, const std::vector<sl_u8> &reserve = std::vector<sl_u8>(), 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 <typename T>
|
||||
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;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -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
|
||||
@@ -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 <cstdint>
|
||||
|
||||
#define SL_DEFINE_TYPE(IntType, NewType) typedef std::IntType NewType
|
||||
#else
|
||||
#include <stdint.h>
|
||||
|
||||
#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) )
|
||||
@@ -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 <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
// libc++ dep
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
// linux specific
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <pthread.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/select.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "timer.h"
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <assert.h>
|
||||
// linux specific
|
||||
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <time.h>
|
||||
#include "hal/types.h"
|
||||
#include "arch/linux/net_serial.h"
|
||||
#include <sys/select.h>
|
||||
|
||||
#include <algorithm>
|
||||
//__GNUC__
|
||||
#if defined(__GNUC__)
|
||||
// for Linux extension
|
||||
#include <asm/ioctls.h>
|
||||
#include <asm/termbits.h>
|
||||
#include <sys/ioctl.h>
|
||||
extern "C" int tcflush(int fildes, int queue_selector);
|
||||
#else
|
||||
// for other standard UNIX
|
||||
#include <termios.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#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<size);
|
||||
|
||||
|
||||
return tx_len;
|
||||
}
|
||||
|
||||
|
||||
int raw_serial::recvdata(unsigned char * data, size_t size)
|
||||
{
|
||||
if (!isOpened()) return 0;
|
||||
|
||||
int ans = ::read(serial_fd, data, size);
|
||||
|
||||
if (ans == -1) ans=0;
|
||||
required_rx_cnt = ans;
|
||||
return ans;
|
||||
}
|
||||
|
||||
|
||||
void raw_serial::flush( _u32 flags)
|
||||
{
|
||||
tcflush(serial_fd,TCIFLUSH);
|
||||
}
|
||||
|
||||
int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
|
||||
{
|
||||
if (returned_size) *returned_size = required_tx_cnt;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
|
||||
{
|
||||
if (!isOpened() ) return -1;
|
||||
|
||||
if (returned_size) *returned_size = required_rx_cnt;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
|
||||
{
|
||||
size_t length = 0;
|
||||
if (returned_size==NULL) returned_size=(size_t *)&length;
|
||||
*returned_size = 0;
|
||||
|
||||
int max_fd;
|
||||
fd_set input_set;
|
||||
struct timeval timeout_val;
|
||||
|
||||
/* Initialize the input set */
|
||||
FD_ZERO(&input_set);
|
||||
FD_SET(serial_fd, &input_set);
|
||||
|
||||
if (_selfpipe[0] != -1)
|
||||
FD_SET(_selfpipe[0], &input_set);
|
||||
|
||||
max_fd = std::max<int>(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
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
}}}
|
||||
@@ -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 <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <netdb.h>
|
||||
|
||||
#include <net/if.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
|
||||
|
||||
|
||||
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<void *>(new sockaddr_storage);
|
||||
memset(_platform_data, 0, sizeof(sockaddr_storage));
|
||||
|
||||
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
|
||||
}
|
||||
|
||||
SocketAddress::SocketAddress(const SocketAddress & src)
|
||||
{
|
||||
_platform_data = reinterpret_cast<void *>(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<void *>(new sockaddr_storage);
|
||||
memset(_platform_data, 0, sizeof(sockaddr_storage));
|
||||
|
||||
// default to ipv4 in case the following operation fails
|
||||
reinterpret_cast<sockaddr_storage *>(_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<sockaddr_storage *>(_platform_data);
|
||||
}
|
||||
|
||||
SocketAddress::address_type_t SocketAddress::getAddressType() const
|
||||
{
|
||||
switch(reinterpret_cast<const sockaddr_storage *>(_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<const sockaddr_in *>(_platform_data)->sin_port);
|
||||
case ADDRESS_TYPE_INET6:
|
||||
return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
u_result SocketAddress::setPort(int port)
|
||||
{
|
||||
switch (getAddressType()) {
|
||||
case ADDRESS_TYPE_INET:
|
||||
reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
|
||||
break;
|
||||
case ADDRESS_TYPE_INET6:
|
||||
reinterpret_cast<sockaddr_in6 *>(_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<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
|
||||
ans = inet_pton(AF_INET,
|
||||
address_string,
|
||||
&reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
|
||||
break;
|
||||
|
||||
|
||||
case ADDRESS_TYPE_INET6:
|
||||
|
||||
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
|
||||
ans = inet_pton(AF_INET6,
|
||||
address_string,
|
||||
&reinterpret_cast<sockaddr_in6 *>(_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<const sockaddr_storage *>(_platform_data)->ss_family;
|
||||
const char *ans = NULL;
|
||||
switch (net_family) {
|
||||
case AF_INET:
|
||||
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
|
||||
buffer, buffersize);
|
||||
break;
|
||||
|
||||
case AF_INET6:
|
||||
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_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<SocketAddress> &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<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_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<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_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<sockaddr_in *>(_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<sockaddr_in6 *>(_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<sockaddr_in *>(_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<sockaddr_in *>(_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<sockaddr_in6 *>(_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<const struct sockaddr *>(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<struct sockaddr *>( const_cast<void *>(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<const struct sockaddr *>(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<struct sockaddr *>(const_cast<void *>(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<struct sockaddr *>(const_cast<void *>(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<const struct sockaddr *>(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<struct sockaddr *>(const_cast<void *>((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<const struct sockaddr *>(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<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&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<struct sockaddr *>(const_cast<void *>(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<struct sockaddr *>(const_cast<void *>(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<StreamSocket *>(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<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
|
||||
return newborn;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}}
|
||||
|
||||
@@ -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 <sched.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/syscall.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/resource.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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}}
|
||||
@@ -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;
|
||||
}
|
||||
}}
|
||||
@@ -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 <unistd.h>
|
||||
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()
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
// libc++ dep
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
|
||||
// POSIX specific
|
||||
extern "C" {
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <pthread.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <time.h>
|
||||
}
|
||||
|
||||
#include "arch/macOS/timer.h"
|
||||
|
||||
@@ -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 <termios.h>
|
||||
#include <sys/select.h>
|
||||
#include <IOKit/serial/ioss.h>
|
||||
|
||||
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<size);
|
||||
|
||||
|
||||
return tx_len;
|
||||
}
|
||||
|
||||
|
||||
int raw_serial::recvdata(unsigned char * data, size_t size)
|
||||
{
|
||||
if (!isOpened()) return 0;
|
||||
|
||||
int ans = ::read(serial_fd, data, size);
|
||||
|
||||
if (ans == -1) ans=0;
|
||||
required_rx_cnt = ans;
|
||||
return ans;
|
||||
}
|
||||
|
||||
|
||||
void raw_serial::flush( _u32 flags)
|
||||
{
|
||||
tcflush(serial_fd,TCIFLUSH);
|
||||
}
|
||||
|
||||
int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
|
||||
{
|
||||
if (returned_size) *returned_size = required_tx_cnt;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
|
||||
{
|
||||
if (!isOpened() ) return -1;
|
||||
|
||||
if (returned_size) *returned_size = required_rx_cnt;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
|
||||
{
|
||||
size_t length = 0;
|
||||
if (returned_size==NULL) returned_size=(size_t *)&length;
|
||||
*returned_size = 0;
|
||||
|
||||
int max_fd;
|
||||
fd_set input_set;
|
||||
struct timeval timeout_val;
|
||||
|
||||
/* Initialize the input set */
|
||||
FD_ZERO(&input_set);
|
||||
FD_SET(serial_fd, &input_set);
|
||||
max_fd = serial_fd + 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
|
||||
{
|
||||
// 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
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
}}}
|
||||
@@ -1,899 +0,0 @@
|
||||
/*
|
||||
* RoboPeak Project
|
||||
* HAL Layer - Socket Interface
|
||||
* Copyright 2018 RoboPeak Project
|
||||
*
|
||||
* macOS Implementation
|
||||
*/
|
||||
|
||||
|
||||
#include "sdkcommon.h"
|
||||
#include "../../hal/socket.h"
|
||||
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <netdb.h>
|
||||
|
||||
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<void *>(new sockaddr_storage);
|
||||
memset(_platform_data, 0, sizeof(sockaddr_storage));
|
||||
|
||||
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
|
||||
}
|
||||
|
||||
SocketAddress::SocketAddress(const SocketAddress & src)
|
||||
{
|
||||
_platform_data = reinterpret_cast<void *>(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<void *>(new sockaddr_storage);
|
||||
memset(_platform_data, 0, sizeof(sockaddr_storage));
|
||||
|
||||
// default to ipv4 in case the following operation fails
|
||||
reinterpret_cast<sockaddr_storage *>(_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<sockaddr_storage *>(_platform_data);
|
||||
}
|
||||
|
||||
SocketAddress::address_type_t SocketAddress::getAddressType() const
|
||||
{
|
||||
switch(reinterpret_cast<const sockaddr_storage *>(_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<const sockaddr_in *>(_platform_data)->sin_port);
|
||||
case ADDRESS_TYPE_INET6:
|
||||
return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
u_result SocketAddress::setPort(int port)
|
||||
{
|
||||
switch (getAddressType()) {
|
||||
case ADDRESS_TYPE_INET:
|
||||
reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
|
||||
break;
|
||||
case ADDRESS_TYPE_INET6:
|
||||
reinterpret_cast<sockaddr_in6 *>(_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<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
|
||||
ans = inet_pton(AF_INET,
|
||||
address_string,
|
||||
&reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
|
||||
break;
|
||||
|
||||
|
||||
case ADDRESS_TYPE_INET6:
|
||||
|
||||
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
|
||||
ans = inet_pton(AF_INET6,
|
||||
address_string,
|
||||
&reinterpret_cast<sockaddr_in6 *>(_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<const sockaddr_storage *>(_platform_data)->ss_family;
|
||||
const char *ans = NULL;
|
||||
switch (net_family) {
|
||||
case AF_INET:
|
||||
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
|
||||
buffer, buffersize);
|
||||
break;
|
||||
|
||||
case AF_INET6:
|
||||
ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_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<SocketAddress> &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<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
|
||||
|
||||
|
||||
break;
|
||||
case ADDRESS_TYPE_INET6:
|
||||
if (bufferSize < sizeof(in6_addr)) return RESULT_INSUFFICIENT_MEMORY;
|
||||
memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_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<sockaddr_in *>(_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<sockaddr_in6 *>(_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<sockaddr_in *>(_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<sockaddr_in *>(_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<sockaddr_in6 *>(_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<const struct sockaddr *>(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<struct sockaddr *>( const_cast<void *>(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<const struct sockaddr *>(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<struct sockaddr *>(const_cast<void *>(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<struct sockaddr *>(const_cast<void *>(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<const struct sockaddr *>(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<struct sockaddr *>(const_cast<void *>((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<const struct sockaddr *>(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<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&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<struct sockaddr *>(const_cast<void *>(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<struct sockaddr *>(const_cast<void *>(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<StreamSocket *>(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<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
|
||||
return newborn;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}}
|
||||
@@ -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 <unistd.h>
|
||||
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()
|
||||
@@ -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 <stddef.h>
|
||||
#include <stdio.h>
|
||||
#include <windows.h>
|
||||
#include <stdlib.h> //for memcpy etc..
|
||||
#include <process.h>
|
||||
#include <direct.h>
|
||||
|
||||
|
||||
#include "timer.h"
|
||||
@@ -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
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
}}}
|
||||
@@ -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 <windows.h>
|
||||
#include <winsock2.h>
|
||||
#include <ws2tcpip.h>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#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<void *>(new sockaddr_storage);
|
||||
memset(_platform_data, 0, sizeof(sockaddr_storage));
|
||||
|
||||
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
|
||||
}
|
||||
|
||||
SocketAddress::SocketAddress(const SocketAddress & src)
|
||||
{
|
||||
_platform_data = reinterpret_cast<void *>(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<void *>(new sockaddr_storage);
|
||||
memset(_platform_data, 0, sizeof(sockaddr_storage));
|
||||
|
||||
// default to ipv4 in case the following operation fails
|
||||
reinterpret_cast<sockaddr_storage *>(_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<sockaddr_storage *>(_platform_data);
|
||||
}
|
||||
|
||||
SocketAddress::address_type_t SocketAddress::getAddressType() const
|
||||
{
|
||||
switch(reinterpret_cast<const sockaddr_storage *>(_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<const sockaddr_in *>(_platform_data)->sin_port);
|
||||
case ADDRESS_TYPE_INET6:
|
||||
return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
u_result SocketAddress::setPort(int port)
|
||||
{
|
||||
switch (getAddressType()) {
|
||||
case ADDRESS_TYPE_INET:
|
||||
reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
|
||||
break;
|
||||
case ADDRESS_TYPE_INET6:
|
||||
reinterpret_cast<sockaddr_in6 *>(_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<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
|
||||
ans = _inet_pton(AF_INET,
|
||||
address_string,
|
||||
&reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
|
||||
break;
|
||||
|
||||
|
||||
case ADDRESS_TYPE_INET6:
|
||||
|
||||
reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
|
||||
ans = _inet_pton(AF_INET6,
|
||||
address_string,
|
||||
&reinterpret_cast<sockaddr_in6 *>(_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<const sockaddr_storage *>(_platform_data)->ss_family;
|
||||
const char *ans = NULL;
|
||||
switch (net_family) {
|
||||
case AF_INET:
|
||||
ans = _inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
|
||||
buffer, (int)buffersize);
|
||||
break;
|
||||
|
||||
case AF_INET6:
|
||||
ans = _inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_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<SocketAddress> &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<const sockaddr_in *>(_platform_data)->sin_addr.s_addr)) return RESULT_INSUFFICIENT_MEMORY;
|
||||
|
||||
memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
|
||||
|
||||
|
||||
break;
|
||||
case ADDRESS_TYPE_INET6:
|
||||
if (bufferSize < sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
|
||||
memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_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<sockaddr_in *>(_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<sockaddr_in6 *>(_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<sockaddr_in *>(_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<sockaddr_in *>(_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<sockaddr_in6 *>(_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<const struct sockaddr *>(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<struct sockaddr *>( const_cast<void *>(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<const struct sockaddr *>(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<struct sockaddr *>(const_cast<void *>(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<struct sockaddr *>(const_cast<void *>(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<const struct sockaddr *>(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<struct sockaddr *>(const_cast<void *>((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<const struct sockaddr *>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr *>(&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<const struct sockaddr *>(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<struct sockaddr *>(const_cast<void *>(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<StreamSocket *>(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<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
|
||||
return newborn;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}}
|
||||
|
||||
@@ -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 <mmsystem.h>
|
||||
#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)
|
||||
|
||||
}}
|
||||
@@ -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()
|
||||
@@ -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 <process.h>
|
||||
|
||||
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<HANDLE>(this->_handle), -1))
|
||||
{
|
||||
CloseHandle(reinterpret_cast<HANDLE>(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<HANDLE>(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_TIME_CRITICAL && win_priority>=THREAD_PRIORITY_ABOVE_NORMAL)
|
||||
{
|
||||
return PRIORITY_HIGH;
|
||||
}
|
||||
else if (win_priority<THREAD_PRIORITY_ABOVE_NORMAL && win_priority>THREAD_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<HANDLE>(this->_handle), timeout))
|
||||
{
|
||||
case WAIT_OBJECT_0:
|
||||
CloseHandle(reinterpret_cast<HANDLE>(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;
|
||||
}
|
||||
|
||||
}}
|
||||
@@ -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 <algorithm>
|
||||
#include <memory>
|
||||
|
||||
#define CONF_NO_BOOST_CRC_SUPPORT
|
||||
|
||||
#include "dataupacker_namespace.h"
|
||||
|
||||
|
||||
@@ -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()
|
||||
@@ -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 <map>
|
||||
|
||||
|
||||
#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<IDataUnpackerHandler *> & 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<const _u8 *>(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<IDataUnpackerHandler*> 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()
|
||||
@@ -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()
|
||||
@@ -1,5 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
#define BEGIN_DATAUNPACKER_NS() namespace sl{ namespace internal{
|
||||
#define END_DATAUNPACKER_NS() }}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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()
|
||||
@@ -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<rplidar_response_hq_capsule_measurement_nodes_t*>(&_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<const SlamtecLidarTimingDesc*>(data);
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackerHandler_HQNode::reset()
|
||||
{
|
||||
_cached_scan_node_buf_pos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -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()
|
||||
@@ -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<rplidar_response_measurement_node_t*>(&_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<const SlamtecLidarTimingDesc*>(data);
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackerHandler_NormalNode::reset()
|
||||
{
|
||||
_cached_scan_node_buf_pos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -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()
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
}}
|
||||
|
||||
|
||||
|
||||
@@ -1,18 +0,0 @@
|
||||
#ifndef _INFRA_HAL_ASSERT_H
|
||||
#define _INFRA_HAL_ASSERT_H
|
||||
|
||||
#ifdef WIN32
|
||||
#include <crtdbg.h>
|
||||
#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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
};
|
||||
}}
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
|
||||
}}
|
||||
|
||||
@@ -1,149 +0,0 @@
|
||||
/*
|
||||
* RoboPeak Project
|
||||
* HAL Layer - Socket Interface
|
||||
* Copyright 2009 - 2013 RoboPeak Project
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
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<SocketAddress> &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() {}
|
||||
};
|
||||
|
||||
}}
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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<c, &c::x>(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 <class T, u_result (T::*PROC)(void)>
|
||||
static Thread create_member(T * pthis)
|
||||
{
|
||||
return create(_thread_thunk<T,PROC>, pthis);
|
||||
}
|
||||
|
||||
template <class T, u_result (T::*PROC)(void) >
|
||||
static _word_size_t THREAD_PROC _thread_thunk(void * data)
|
||||
{
|
||||
return (static_cast<T *>(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;
|
||||
};
|
||||
|
||||
}}
|
||||
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
#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
|
||||
@@ -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 <typename _CountofType, size_t _SizeOfArray>
|
||||
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_;
|
||||
|
||||
@@ -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<typename ResultT>
|
||||
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
|
||||
@@ -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 <algorithm>
|
||||
|
||||
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<RplidarScanMode>& 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);
|
||||
}
|
||||
|
||||
}}}
|
||||
@@ -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"
|
||||
@@ -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<nullptr_t> 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"<<endl;
|
||||
_codec.onDecodeData(bufferToDecode->data, bufferToDecode->size);
|
||||
|
||||
|
||||
delete bufferToDecode;
|
||||
}
|
||||
|
||||
return RESULT_OK;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}}
|
||||
@@ -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 <list>
|
||||
#include <memory>
|
||||
|
||||
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<ProtocolMessage> 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;
|
||||
};
|
||||
|
||||
|
||||
}}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user