Use upstream lidar code
This commit is contained in:
+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;
|
||||
};
|
||||
|
||||
|
||||
}}
|
||||
@@ -1,102 +0,0 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "sl_crc.h"
|
||||
|
||||
namespace sl {namespace crc32 {
|
||||
|
||||
static sl_u32 table[256];//crc32_table
|
||||
sl_u32 bitrev(sl_u32 input, sl_u16 bw)
|
||||
{
|
||||
sl_u16 i;
|
||||
sl_u32 var;
|
||||
var = 0;
|
||||
for (i = 0; i < bw; i++) {
|
||||
if (input & 0x01) {
|
||||
var |= 1 << (bw - 1 - i);
|
||||
}
|
||||
input >>= 1;
|
||||
}
|
||||
return var;
|
||||
}
|
||||
|
||||
void init(sl_u32 poly)
|
||||
{
|
||||
sl_u16 i;
|
||||
sl_u16 j;
|
||||
sl_u32 c;
|
||||
|
||||
poly = bitrev(poly, 32);
|
||||
for (i = 0; i < 256; i++) {
|
||||
c = i;
|
||||
for (j = 0; j < 8; j++) {
|
||||
if (c & 1)
|
||||
c = poly ^ (c >> 1);
|
||||
else
|
||||
c = c >> 1;
|
||||
}
|
||||
table[i] = c;
|
||||
}
|
||||
}
|
||||
|
||||
sl_u32 cal(sl_u32 crc, void* input, sl_u16 len)
|
||||
{
|
||||
sl_u16 i;
|
||||
sl_u8 index;
|
||||
sl_u8* pch;
|
||||
pch = (unsigned char*)input;
|
||||
sl_u8 leftBytes = 4 - (len & 0x3);
|
||||
|
||||
for (i = 0; i < len; i++) {
|
||||
index = (unsigned char)(crc^*pch);
|
||||
crc = (crc >> 8) ^ table[index];
|
||||
pch++;
|
||||
}
|
||||
|
||||
for (i = 0; i < leftBytes; i++) {//zero padding
|
||||
index = (unsigned char)(crc ^ 0);
|
||||
crc = (crc >> 8) ^ table[index];
|
||||
}
|
||||
return crc ^ 0xffffffff;
|
||||
}
|
||||
|
||||
sl_result getResult(sl_u8 *ptr, sl_u32 len)
|
||||
{
|
||||
static sl_u8 tmp;
|
||||
if (tmp != 1) {
|
||||
init(0x4C11DB7);
|
||||
tmp = 1;
|
||||
}
|
||||
|
||||
return cal(0xFFFFFFFF, ptr, len);
|
||||
}
|
||||
}}
|
||||
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user