hongshaorou/ros2bookcode
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 更新第九章

Browse Source
This commit is contained in:
fishros
2025-02-22 20:45:21 +08:00
parent 497be290f4
commit 2738b34dc1
225 changed files with 42292 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt
View File

@@ -22,6 +22,7 @@ if(BUILD_TESTING)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
@@ -34,8 +35,9 @@ ament_target_dependencies(odom2tf
)
install(TARGETS odom2tf
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
ament_package()

1
chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py
View File

@@ -41,6 +41,7 @@ def generate_launch_description():
# 使用 TimerAction 启动后 5 秒执行 ydlidar 节点
ydlidar_delay = launch.actions.TimerAction(period=5.0, actions=[ydlidar])
return launch.LaunchDescription([
urdf2tf,
odom2tf,

BIN
chapt9/fishbot_ws/src/fishbot_bringup/map/room.pgm Normal file
View File

Binary file not shown.

7
chapt9/fishbot_ws/src/fishbot_bringup/map/room.yaml Normal file
View File

@@ -0,0 +1,7 @@
image: room.pgm
mode: trinary
resolution: 0.05
origin: [-1.23, -1.95, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

2
chapt9/fishbot_ws/src/fishbot_bringup/package.xml
View File

@@ -4,7 +4,7 @@
<name>fishbot_bringup</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
<maintainer email="fishros@foxmail.com">fishros</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>

6
chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt
View File

@@ -23,9 +23,9 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
install(DIRECTORY
urdf
DESTINATION share/${PROJECT_NAME}
install(DIRECTORY
urdf
DESTINATION share/${PROJECT_NAME}
)
ament_package()

2
chapt9/fishbot_ws/src/fishbot_description/package.xml
View File

@@ -4,7 +4,7 @@
<name>fishbot_description</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
<maintainer email="fishros@foxmail.com">fishros</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>

1
chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf
View File

@@ -14,6 +14,7 @@
</material>
</visual>
</link>
<joint name="base_joint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />

1
chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt
View File

@@ -30,4 +30,5 @@ install(DIRECTORY
DESTINATION share/${PROJECT_NAME}
)
ament_package()

1
chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml
View File

@@ -47,6 +47,7 @@ bt_navigator:
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 1000
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

BIN
chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.pgm
View File

Binary file not shown.

2
chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.yaml
View File

@@ -1,7 +1,7 @@
image: room.pgm
mode: trinary
resolution: 0.05
origin: [-3.9, -1.82, 0]
origin: [-1.23, -1.95, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

2
chapt9/fishbot_ws/src/fishbot_navigation2/package.xml
View File

@@ -4,7 +4,7 @@
<name>fishbot_navigation2</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
<maintainer email="fishros@foxmail.com">fishros</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>

1
chapt9/fishbot_ws/src/micro-ROS-Agent Submodule

Submodule chapt9/fishbot_ws/src/micro-ROS-Agent added at 30377bbd86

1
chapt9/fishbot_ws/src/micro_ros_msgs Submodule

Submodule chapt9/fishbot_ws/src/micro_ros_msgs added at 10be4d005f

3
chapt9/fishbot_ws/src/ros_serial2wifi/.gitignore vendored Normal file
View File

@@ -0,0 +1,3 @@
build/
log/
install/

18
chapt9/fishbot_ws/src/ros_serial2wifi/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?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>ros_serail2wifi</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
chapt9/fishbot_ws/src/ros_serial2wifi/resource/ros_serail2wifi Normal file
View File

0
chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/__init__.py Normal file
View File

80
chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/tcpserver.py Normal file
View File

@@ -0,0 +1,80 @@
#!/usr/bin/env python3
import os
import pty
import socket
import select
import subprocess
import rclpy
from rclpy.node import Node
import time
class TcpSocketServerNode(Node):
def __init__(self):
super().__init__('tcp_socket_server_node')
# 声明 ROS 2 参数
self.declare_parameter('tcp_port', 8889)
self.declare_parameter('serial_port', '/tmp/laserport')
# 获取 ROS 2 参数
self.tcp_port = self.get_parameter('tcp_port').get_parameter_value().integer_value
self.serial_port = self.get_parameter('serial_port').get_parameter_value().string_value
def run(self):
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(('0.0.0.0', self.tcp_port))
s.listen(1)
master, slave = pty.openpty()
if os.path.exists(self.serial_port):
os.remove(self.serial_port)
os.symlink(os.ttyname(slave), self.serial_port)
self.get_logger().info(f"TCP端口:{self.tcp_port}，已映射到串口设备:{self.serial_port}")
mypoll = select.poll()
mypoll.register(master, select.POLLIN)
try:
while True:
self.get_logger().info("等待接受连接..")
s.settimeout(None)
client, client_address = s.accept()
mypoll.register(client.fileno(), select.POLLIN)
self.get_logger().info(f'来自{client_address}的连接已建立')
is_connect = True
last_exchange_data_time = time.time()
try:
while is_connect:
fdlist = mypoll.poll(256)
for fd, event in fdlist:
last_exchange_data_time = time.time()
data = os.read(fd, 256)
write_fd = client.fileno() if fd == master else master
if len(data) == 0:
is_connect = False
break
# print(write_fd,data,event)
os.write(write_fd, data)
# 如果一段时间没有任何数据则断开连接
if time.time()-last_exchange_data_time>5:
is_connect = False
print('5s no data.')
break
except Exception:
is_connect = False
finally:
mypoll.unregister(client.fileno())
client.close()
finally:
s.close()
def main():
rclpy.init()
node = TcpSocketServerNode()
node.run()
rclpy.shutdown()
if __name__ == "__main__":
main()

4
chapt9/fishbot_ws/src/ros_serial2wifi/setup.cfg Normal file
View File

@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/ros_serail2wifi
[install]
install_scripts=$base/lib/ros_serail2wifi

26
chapt9/fishbot_ws/src/ros_serial2wifi/setup.py Normal file
View File

@@ -0,0 +1,26 @@
from setuptools import find_packages, setup
package_name = 'ros_serail2wifi'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='fishros',
maintainer_email='87068644+fishros@users.noreply.github.com',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'tcp_server=ros_serail2wifi.tcpserver:main'
],
},
)

25
chapt9/fishbot_ws/src/ros_serial2wifi/test/test_copyright.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'

25
chapt9/fishbot_ws/src/ros_serial2wifi/test/test_flake8.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)

23
chapt9/fishbot_ws/src/ros_serial2wifi/test/test_pep257.py Normal file
View File

@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'

3
chapt9/fishbot_ws/src/ydlidar_ros2/.gitignore vendored Normal file
View File

@@ -0,0 +1,3 @@
build/*
install/*
log/*

137
chapt9/fishbot_ws/src/ydlidar_ros2/CMakeLists.txt Normal file
View File

@@ -0,0 +1,137 @@
# Copyright(c) 2020 eaibot limited.
cmake_minimum_required(VERSION 3.5)
project(ydlidar C CXX)
##################ros2#############################################
# Default to C++14
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()
# add_subdirectory(sdk)
#########################################################
set(YDLIDAR_SDK_VERSION_MAJOR 1)
set(YDLIDAR_SDK_VERSION_MINOR 2)
set(YDLIDAR_SDK_VERSION_PATCH 9)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
set(YDSDK_NAME "ydsdk")
##########################################################
# Detect wordsize:
IF(CMAKE_SIZEOF_VOID_P EQUAL 8) # Size in bytes!
SET(CMAKE_MRPT_WORD_SIZE 64)
ELSE()
SET(CMAKE_MRPT_WORD_SIZE 32)
ENDIF()
#####################################################
# add cmake module path
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
set(SDK_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
#############################################################################
# include cmake file
include(common/ydlidar_base)
############################################################################
# include headers
include_directories(.)
include_directories(sdk/core)
include_directories(sdk/src)
include_directories(sdk/core/common)
#############################################################################
# addd subdirectory
add_subdirectory(sdk/core)
add_subdirectory(sdk/src)
#############################################################################
# PARSE libraries
include(common/ydlidar_parse)
include_directories(${SDK_INCS})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
####################find package#####################################
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rmw REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED)
############## YDLIDAR SDK START#####################################
#find ydlidar_sdk package
# find_package(ydlidar_sdk REQUIRED)
############## YDLIDAR SDK END#####################################
#Include directories
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/sdk
${PROJECT_SOURCE_DIR}/src
${YDLIDAR_SDK_INCLUDE_DIRS})
#link library directories
link_directories(${YDLIDAR_SDK_LIBRARY_DIRS})
#---------------------------------------------------------------------------------------
# generate excutable and add libraries
#---------------------------------------------------------------------------------------
add_executable(${PROJECT_NAME}_node
src/${PROJECT_NAME}_node.cpp ${SDK_SOURCES} ${SDK_HEADERS} ${GENERATED_HEADERS})
# add_dependencies(${PROJECT_NAME}_node ydsdk)
#---------------------------------------------------------------------------------------
# link libraries
#--------------------------------------------------------------------------------------
ament_target_dependencies(${PROJECT_NAME}_node
"rclcpp"
"sensor_msgs"
"visualization_msgs"
"geometry_msgs"
"std_srvs"
)
target_link_libraries(${PROJECT_NAME}_node
${YDLIDAR_SDK_LIBRARIES})
#---------------------------------------------------------------------------------------
# generate excutable and add libraries
#---------------------------------------------------------------------------------------
add_executable(${PROJECT_NAME}_client
src/${PROJECT_NAME}_client.cpp)
#---------------------------------------------------------------------------------------
# link libraries
#--------------------------------------------------------------------------------------
ament_target_dependencies(${PROJECT_NAME}_client
"rclcpp"
"sensor_msgs"
"visualization_msgs"
"geometry_msgs"
"std_srvs"
)
#---------------------------------------------------------------------------------------
# Install
#---------------------------------------------------------------------------------------
install(TARGETS
${PROJECT_NAME}_node ${PROJECT_NAME}_client
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch params startup config
DESTINATION share/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

268
chapt9/fishbot_ws/src/ydlidar_ros2/README.md Normal file
View File

@@ -0,0 +1,268 @@
![YDLIDAR](sdk/image/YDLidar.jpg "YDLIDAR")
# YDLIDAR ROS2 PACKAGE V1.4.5
ROS2 node and test application for YDLIDAR
Visit EAI Website for more details about YDLIDAR.
## How to [install ROS2](https://index.ros.org/doc/ros2/Installation)
[ubuntu](https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/)
[windows](https://index.ros.org/doc/ros2/Installation/Dashing/Windows-Install-Binary/)
## How to Create a ROS2 workspace
[Create a workspace](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#create-a-workspace)
## How to build YDLIDAR ros2 package
1) Clone this project to your ament's workspace src folder
2) Running ament to build ydlidar_node and ydlidar_client
3) Create the name "/dev/ydlidar" for YDLIDAR
--$ cd workspace/ydlidar_ros2/startup
--$ sudo chmod 777 ./*
--$ sudo sh initenv.sh
Note: Download and Build details [here](docs/ydlidar.md)
## How to run YDLIDAR ros2 package
### 1. Run YDLIDAR node and view using test application
$ros2 run ydlidar ydlidar_node
$ros2 run ydlidar ydlidar_client
Note: You should see YDLIDAR's scan result in the console
### 2.Run YDLIDAR node and view using test application by launch
$launch $(ros2 pkg prefix ydlidar)/share/ydlidar/launch/ydlidar.py
$ros2 run ydldiar ydlidar_client or ros2 topic echo /scan
or
$ros2 launch ydlidar ydlidar_launch.py
## Dataset
|LIDAR | Model | Baudrate | SampleRate(K) | Range(m) | Frequency(HZ) | Intenstiy(bit) | SingleChannel | voltage(V)|
| :-------- |:--:|:--:|:--:|:--:|:--:|:--:|:--:|:--:|
| F4 | 1 | 115200 | 4 | 0.12~12 | 5~12 | false | false | 4.8~5.2 |
| S4 | 4 | 115200 | 4 | 0.10~8.0 | 5~12 (PWM) | false | false | 4.8~5.2 |
| S4B | 4/11 | 153600 | 4 | 0.10~8.0 | 5~12(PWM) | true(8) | false | 4.8~5.2 |
| S2 | 4/12 | 115200 | 3 | 0.10~8.0 | 4~8(PWM) | false | true | 4.8~5.2 |
| G4 | 5 | 230400 | 9/8/4 | 0.28/0.26/0.1~16| 5~12 | false | false | 4.8~5.2 |
| X4 | 6 | 128000 | 5 | 0.12~10 | 5~12(PWM) | false | false | 4.8~5.2 |
| X2/X2L | 6 | 115200 | 3 | 0.10~8.0 | 4~8(PWM) | false | true | 4.8~5.2 |
| G4PRO | 7 | 230400 | 9/8/4 | 0.28/0.26/0.1~16| 5~12 | false | false | 4.8~5.2 |
| F4PRO | 8 | 230400 | 4/6 | 0.12~12 | 5~12 | false | false | 4.8~5.2 |
| R2 | 9 | 230400 | 5 | 0.12~16 | 5~12 | false | false | 4.8~5.2 |
| G6 | 13 | 512000 | 18/16/8 | 0.28/0.26/0.1~25| 5~12 | false | false | 4.8~5.2 |
| G2A | 14 | 230400 | 5 | 0.12~12 | 5~12 | false | false | 4.8~5.2 |
| G2 | 15 | 230400 | 5 | 0.28~16 | 5~12 | true(8) | false | 4.8~5.2 |
| G2C | 16 | 115200 | 4 | 0.1~12 | 5~12 | false | false | 4.8~5.2 |
| G4B | 17 | 512000 | 10 | 0.12~16 | 5~12 | true(10) | false | 4.8~5.2 |
| G4C | 18 | 115200 | 4 | 0.1~12 | 5~12 | false | false | 4.8~5.2 |
| G1 | 19 | 230400 | 9 | 0.28~16 | 5~12 | false | false | 4.8~5.2 |
| TX8 　 | 100 | 115200 | 4 | 0.1~8 | 4~8(PWM) | false | true | 4.8~5.2 |
| TX20 　| 100 | 115200 | 4 | 0.1~20 | 4~8(PWM) | false | true | 4.8~5.2 |
| TG15 　| 100 | 512000 | 20/18/10 | 0.05~15 | 3~16 | false | false | 4.8~5.2 |
| TG30 　| 101 | 512000 | 20/18/10 | 0.05~30 | 3~16 | false | false | 4.8~5.2 |
| TG50 　| 102 | 512000 | 20/18/10 | 0.05~50 | 3~16 | false | false | 4.8~5.2 |
Note: PWM option speed control requires external PWM wave.
## Configuration
path: [ydlidar.yaml](params/ydlidar.yaml)
## ros2-interfaces
<center>
| Topic | Type | Description |
|----------------------|-------------------------|--------------------------------------------------|
| `scan` | sensor_msgs/LaserScan | 2D laser scan of the 0-angle ring |
| Parameter | Type | Description |
|-----------------------|------------------------|-----------------------------------------------------|
| `port` | String | port of lidar (ex. /dev/ttyUSB0) |
| `baudrate` | int | baudrate of lidar (ex. 230400) |
| `frame_id` | String | TF frame of sensor, default: `laser_frame` |
| `singleChannel` | bool | Whether LiDAR is a single-channel, default: false |
| `resolution_fixed` | bool | Fixed angluar resolution, default: true |
| `auto_reconnect` | bool | Automatically reconnect the LiDAR, default: true |
| `reversion` | bool | Reversion LiDAR, default: true |
| `isToFLidar` | bool | Whether LiDAR is TOF Type, default: false |
| `angle_min` | float | Minimum Valid Angle, defalut: -180.0 |
| `angle_max` | float | Maximum Valid Angle, defalut: 180.0 |
| `range_min` | float | Minimum Valid range, defalut: 0.01m |
| `range_max` | float | Maximum Valid range, defalut: 64.0m |
| `ignore_array` | String | LiDAR filtering angle area, default: "" |
| `samp_rate` | int | sampling rate of lidar, default: 9 |
| `frequency` | float | scan frequency of lidar,default: 10.0 |
</center>
## Parameters
port (string, default: /dev/ydlidar)
serial port name used in your system.
baudrate (int, default: 230400)
serial port baud rate.
| LiDAR | baudrate |
|-----------------------------------------------|-----------------------|
|F4/S2/X2/X2L/S4/TX8/TX20/G4C | 115200 |
|X4 | 128000 |
|S4B | 153600 |
|G1/G2/R2/G4/G4PRO/F4PRO | 230400 |
|G6/TG15/TG30/TG50 | 512000 |
frame_id (string, default: laser_frame)
frame ID for the device.
singleChannel (bool, default: false)
indicated whether the LIDAR is single communication(S2) lidar.
| LiDAR | singleChannel |
|-----------------------------------------------------------|-----------------------|
|G1/G2/G4/G6/F4/F4PRO/S4/S4B/X4/R2/G4C | false |
|S2/X2/X2L | true |
|TG15/TG30/TG50 | false |
|TX8/TX20 | true |
resolution_fixed (bool, default: true)
indicated whether the LIDAR has a fixed angular resolution.
auto_reconnect (bool, default: true)
indicated whether the LIDAR auto reconnection.
reversion (bool, default: false)
indicated whether the LIDAR data rotation 180°.
| LiDAR | reversion |
|-----------------------------------------------------------------|-----------------------|
|G1/G2/G4/G6/F4/F4PRO//R2/G4C/TG15/TG30/TG50 | true |
|S2/X2/X2L/S4/S4B/X4/TX8/TX20 | false |
isToFLidar (bool, default: false)
indicated whether the LIDAR is TOF(TX8) lidar.
| LiDAR | isToFLidar |
|-----------------------------------------------------------------------|-----------------------|
|G1/G2/G4/G6/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L | false |
|TG15/TG30/TG50/TX8/TX20 | true |
angle_min (double, default: -180)
Min valid angle (°) for LIDAR data.
angle_max (double, default: 180)
Max valid angle (°) for LIDAR data.
range_min (double, default: 0.08)
Min valid range (m) for LIDAR data.
range_max (double, default: 32.0)
Max valid range (m) for LIDAR data.
ignore_array (string, default: "")
Set the current angle range value to zero.
Note: ignore 10 to 20 and 50 to 80, ex: "10, 20, 50, 80"
samp_rate (int, default: 9)
the LIDAR sampling frequency.
| LiDAR | samp_rate |
|-----------------------------|------------------------|
|G4/F4 | 4,8,9 |
|F4PRO | 4,6 |
|G6 | 8,16,18 |
|G1/G2/R2/X4 | 5 |
|S4/S4B/G4C/TX8/TX20 |4 |
|S2 | 3 |
|TG15/TG30/TG50 | 10,18,20 |
frequency (double, default: 10)
the LIDAR scanning frequency.
Note: Specific LiDAR paramter configuration, refer to [Dataset](#dataset)
## Upgrade Log
2020-02-08 version:1.4.5
1.Update SDK to 1.4.5
2.fixed ROS2 Dashing and Eloquent.
2019-12-23 version:1.4.4
1.support all standards lidar
2018-07-16 version:1.3.6
1.Update SDK verison to 1.3.9
2.remove imu sync.
2018-07-16 version:1.3.5
1.Update SDK verison to 1.3.6
2.add imu sync.
2018-04-16 version:1.3.1
1.Update SDK verison to 1.3.1
2.Increase sampling frequency,scan frequency setting.
3.Unified coordinate system.
4.Repair X4,S4 LIDAR cannot be opened.
5.Increased G4 G4C F4Pro LIDAR power-off protection.
6.Increased S4B LIDAR low optical power setting.
7.Fix the wait time for closing ros node.
8.Compensate for each laser point timestamp.
9.Unified profile, automatic correction lidar model.
# 6 Support
You can get support from YDLidar with the following methods:
* Send email to support@ydlidar.com with a clear description of your problem and your setup
* Github Issues
## Contact EAI
![Development Path](sdk/image/EAI.png)
If you have any extra questions, please feel free to [contact us](http://www.ydlidar.cn/cn/contact)

10
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/FindPackage.cmake.in Normal file
View File

@@ -0,0 +1,10 @@
SET( @PACKAGE_PKG_NAME@_LIBRARIES @PACKAGE_LINK_LIBS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ libraries" FORCE )
SET( @PACKAGE_PKG_NAME@_INCLUDE_DIRS @PACKAGE_INCLUDE_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ include directories" FORCE )
SET( @PACKAGE_PKG_NAME@_LIBRARY_DIRS @PACKAGE_LINK_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ library directories" FORCE )
mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARIES )
mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARY_DIRS )
mark_as_advanced( @PACKAGE_PKG_NAME@_INCLUDE_DIRS )

24
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfig.cmake.in Normal file
View File

@@ -0,0 +1,24 @@
SET( @PACKAGE_PKG_NAME@_LIBRARIES "@PACKAGE_LINK_LIBS@" CACHE INTERNAL "@PACKAGE_PKG_NAME@ libraries" FORCE )
SET( @PACKAGE_PKG_NAME@_INCLUDE_DIRS @PACKAGE_INCLUDE_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ include directories" FORCE )
SET( @PACKAGE_PKG_NAME@_LIBRARY_DIRS @PACKAGE_LINK_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ library directories" FORCE )
mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARIES )
mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARY_DIRS )
mark_as_advanced( @PACKAGE_PKG_NAME@_INCLUDE_DIRS )
# Compute paths
get_filename_component( PACKAGE_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH )
# This file, when used for INSTALLED code, does not use Targets... sigh.
## Library dependencies (contains definitions for IMPORTED targets)
#if(NOT TARGET "@PACKAGE_PKG_NAME@_LIBRARIES" AND NOT "@PACKAGE_PKG_NAME@_BINARY_DIR")
# include( "${PACKAGE_CMAKE_DIR}/@PACKAGE_PKG_NAME@Targets.cmake" )
# include( "${PACKAGE_CMAKE_DIR}/@PACKAGE_PKG_NAME@ConfigVersion.cmake" )
#endif()
#SET(@PACKAGE_PKG_NAME@_LIBRARIES @PACKAGE_LIBRARIES@)
#SET(@PACKAGE_PKG_NAME@_LIBRARY @PACKAGE_LIBRARY@)
#SET(@PACKAGE_PKG_NAME@_INCLUDE_DIRS @PACKAGE_INCLUDE_DIRS@)
#SET(@PACKAGE_PKG_NAME@_LINK_DIRS @PACKAGE_LINK_DIRS@)

17
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfigVersion.cmake.in Normal file
View File

@@ -0,0 +1,17 @@
set(PACKAGE_VERSION "@PACKAGE_VERSION@")
# Check build type is valid
if( "System:${CMAKE_SYSTEM_NAME},Android:${ANDROID},iOS:${IOS}" STREQUAL
"System:@CMAKE_SYSTEM_NAME@,Android:@ANDROID@,iOS:@IOS@" )
# Check whether the requested PACKAGE_FIND_VERSION is compatible
if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
set(PACKAGE_VERSION_COMPATIBLE FALSE)
else()
set(PACKAGE_VERSION_COMPATIBLE TRUE)
if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
set(PACKAGE_VERSION_EXACT TRUE)
endif()
endif()
else()
set(PACKAGE_VERSION_COMPATIBLE FALSE)
endif()

11
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PkgConfig.pc.in Normal file
View File

@@ -0,0 +1,11 @@
prefix=@CMAKE_INSTALL_PREFIX@
exec_prefix=${prefix}
libdir=${exec_prefix}/lib
includedir=${prefix}/include
Name: @PACKAGE_PKG_NAME@
Description: @PACKAGE_DESCRIPTION@
Version: @PACKAGE_VERSION@
Cflags: @PACKAGE_CFLAGS@
Libs: -L${libdir} @PACKAGE_LIBS@ @PACKAGE_LIB_LINK@

26
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/cmake_uninstall.cmake.in Normal file
View File

@@ -0,0 +1,26 @@
## A simple uninstall script.
## Alternatively UNIX users can run/sudo `xargs rm < install_manifest.txt` in the build directory.
set(unfile ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
file(WRITE ${unfile} "IF(NOT EXISTS \"install_manifest.txt\")\n")
file(APPEND ${unfile} "MESSAGE(\"FATAL_ERROR Cannot find \\\"install manifest\\\": install_manifest.txt\")\n")
file(APPEND ${unfile} "ENDIF(NOT EXISTS \"install_manifest.txt\")\n")
file(APPEND ${unfile} "FILE(READ \"install_manifest.txt\" files)\n")
file(APPEND ${unfile} "STRING(REGEX REPLACE \"\\n\" \";\" files \"\${files}\")\n")
file(APPEND ${unfile} "FOREACH(file \${files})\n")
file(APPEND ${unfile} " MESSAGE(STATUS \"Uninstalling \\\"\${file}\\\"\")\n")
file(APPEND ${unfile} " IF(EXISTS \"\${file}\")\n")
file(APPEND ${unfile} " EXEC_PROGRAM(\n")
file(APPEND ${unfile} " \"\${CMAKE_COMMAND}\" ARGS \"-E remove \\\"\${file}\\\"\"\n")
file(APPEND ${unfile} " OUTPUT_VARIABLE rm_out\n")
file(APPEND ${unfile} " RETURN_VALUE rm_retval\n")
file(APPEND ${unfile} " )\n")
file(APPEND ${unfile} " IF(\"\${rm_retval}\" STREQUAL 0\)\n")
file(APPEND ${unfile} " ELSE(\"\${rm_retval}\" STREQUAL 0\)\n")
file(APPEND ${unfile} " MESSAGE(FATAL_ERROR \"Problem when removing \\\"\${file}\\\"\")\n")
file(APPEND ${unfile} " ENDIF(\"\${rm_retval}\" STREQUAL 0)\n")
file(APPEND ${unfile} " ELSE(EXISTS \"\${file}\")\n")
file(APPEND ${unfile} " MESSAGE(STATUS \"File \\\"\${file}\\\" does not exist. \")\n")
file(APPEND ${unfile} " ENDIF(EXISTS \"\${file}\")\n")
file(APPEND ${unfile} "ENDFOREACH(file)\n")

388
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_base.cmake Normal file
View File

@@ -0,0 +1,388 @@
include(CMakeParseArguments)
#=============================================================================
#
# ydlidar_parse_function_args
#
# This function simplifies usage of the cmake_parse_arguments module.
# It is intended to be called by other functions.
#
# Usage:
# ydlidar_parse_function_args(
# NAME <name>
# [ OPTIONS <list> ]
# [ ONE_VALUE <list> ]
# [ MULTI_VALUE <list> ]
# REQUIRED <list>
# ARGN <ARGN>)
#
# Input:
# NAME : the name of the calling function
# OPTIONS : boolean flags
# ONE_VALUE : single value variables
# MULTI_VALUE : multi value variables
# REQUIRED : required arguments
# ARGN : the function input arguments, typically ${ARGN}
#
# Output:
# The function arguments corresponding to the following are set:
# ${OPTIONS}, ${ONE_VALUE}, ${MULTI_VALUE}
#
# Example:
# function test()
# ydlidar_parse_function_args(
# NAME TEST
# ONE_VALUE NAME
# MULTI_VALUE LIST
# REQUIRED NAME LIST
# ARGN ${ARGN})
# message(STATUS "name: ${NAME}")
# message(STATUS "list: ${LIST}")
# endfunction()
#
# test(NAME "hello" LIST a b c)
#
# OUTPUT:
# name: hello
# list: a b c
#
function(ydlidar_parse_function_args)
cmake_parse_arguments(IN "" "NAME" "OPTIONS;ONE_VALUE;MULTI_VALUE;REQUIRED;ARGN" "${ARGN}")
cmake_parse_arguments(OUT "${IN_OPTIONS}" "${IN_ONE_VALUE}" "${IN_MULTI_VALUE}" "${IN_ARGN}")
if (OUT_UNPARSED_ARGUMENTS)
message(FATAL_ERROR "${IN_NAME}: unparsed ${OUT_UNPARSED_ARGUMENTS}")
endif()
foreach(arg ${IN_REQUIRED})
if (NOT OUT_${arg})
if (NOT "${OUT_${arg}}" STREQUAL "0")
message(FATAL_ERROR "${IN_NAME} requires argument ${arg}\nARGN: ${IN_ARGN}")
endif()
endif()
endforeach()
foreach(arg ${IN_OPTIONS} ${IN_ONE_VALUE} ${IN_MULTI_VALUE})
set(${arg} ${OUT_${arg}} PARENT_SCOPE)
endforeach()
endfunction()
macro( add_to_ydlidar_include_dirs )
foreach( dir ${ARGN} )
set_property( GLOBAL APPEND PROPERTY YDLIDAR_INCLUDE_DIRS "${dir}" )
endforeach()
endmacro()
macro( add_to_ydlidar_libraries )
foreach( lib ${ARGN} )
# Process targets correctly
if (TARGET ${lib})
# If the library is NOT imported, ie is in this project, we
# want to depend on it directly rather than through its path
get_target_property(is_lib_imported ${lib} IMPORTED)
if (NOT ${is_lib_imported})
set_property( GLOBAL APPEND PROPERTY YDLIDAR_LIBRARIES "${lib}" )
else()
# For imported targets, we just want to depend on the library directly
get_target_property(libpath ${lib} LOCATION)
if (libpath)
set_property( GLOBAL APPEND PROPERTY YDLIDAR_LIBRARIES "${libpath}" )
# This shouldn't really happen, but let's cover our bases.
else()
set_property( GLOBAL APPEND PROPERTY YDLIDAR_LIBRARIES "${lib}" )
endif()
endif()
else() # Just add the direct path/flag to the list
set_property( GLOBAL APPEND PROPERTY YDLIDAR_LIBRARIES "${lib}" )
endif()
endforeach()
endmacro()
macro( add_to_ydlidar_sources )
if("${SDK_SOURCE_DIR}" STREQUAL "")
file(RELATIVE_PATH _relPath "${CMAKE_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
else()
file(RELATIVE_PATH _relPath "${SDK_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
endif()
foreach(_src ${ARGN})
if(_relPath)
set_property( GLOBAL APPEND PROPERTY YDLIDAR_SOURCES "${_relPath}/${_src}" )
else()
set_property( GLOBAL APPEND PROPERTY YDLIDAR_SOURCES "${_src}" )
endif()
endforeach()
endmacro()
macro( add_to_ydlidar_headers )
if("${SDK_SOURCE_DIR}" STREQUAL "")
file(RELATIVE_PATH _relPath "${CMAKE_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
else()
file(RELATIVE_PATH _relPath "${SDK_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
endif()
foreach(_hdr ${ARGN})
if(_relPath)
set_property( GLOBAL APPEND PROPERTY YDLIDAR_HEADERS "${_relPath}/${_hdr}" )
else()
set_property( GLOBAL APPEND PROPERTY YDLIDAR_HEADERS "${_hdr}" )
endif()
endforeach()
endmacro()
macro( ydlidar_set_compile_flags file flags )
set_property( GLOBAL APPEND PROPERTY COMPILER_OPTS_SOURCES "${file}" )
set_property( GLOBAL APPEND PROPERTY COMPILER_OPTS_FLAGS "${flags}" )
endmacro()
macro(subdirlist result curdir)
file(GLOB children RELATIVE ${curdir} ${curdir}/*)
set(dirlist "")
foreach(child ${children})
if( NOT child STREQUAL "CMakeFiles" )
if(IS_DIRECTORY ${curdir}/${child})
set(dirlist ${dirlist} ${child})
endif()
endif()
endforeach()
set(${result} ${dirlist})
endmacro()
macro(aux_include_directory dir result )
set(curdir ${CMAKE_CURRENT_SOURCE_DIR})
if(NOT (${dir} STREQUAL ".") )
set(curdir ${dir})
endif()
FILE(GLOB INC_LIST "${curdir}/*.h")
FILE(GLOB PRV_INC_LIST "${curdir}/*.hpp")
set(INCS "")
foreach(child ${INC_LIST})
string(REPLACE "${curdir}/" "" child_LIST ${child})
list(APPEND INCS ${child_LIST})
endforeach()
foreach(prvchild ${PRV_INC_LIST})
string(REPLACE "${curdir}/" "" prvchild_LIST ${prvchild})
list(APPEND INCS ${prvchild_LIST})
endforeach()
set(${result} ${INCS})
endmacro()
macro(aux_src_directory dir result )
set(curdir ${CMAKE_CURRENT_SOURCE_DIR})
if(NOT (${dir} STREQUAL ".") )
set(curdir ${dir})
endif()
FILE(GLOB INC_LIST "${curdir}/*.c")
FILE(GLOB PRV_INC_LIST "${curdir}/*.cpp")
FILE(GLOB TPP_INC_LIST "${curdir}/*.tpp")
set(INCS "")
foreach(child ${INC_LIST})
string(REPLACE "${curdir}/" "" child_LIST ${child})
list(APPEND INCS ${child_LIST})
endforeach()
foreach(prvchild ${PRV_INC_LIST})
string(REPLACE "${curdir}/" "" prvchild_LIST ${prvchild})
list(APPEND INCS ${prvchild_LIST})
endforeach()
foreach(tppchild ${TPP_INC_LIST})
string(REPLACE "${curdir}/" "" tppchild_LIST ${tppchild})
list(APPEND INCS ${tppchild_LIST})
endforeach()
set(${result} ${INCS})
endmacro()
#=============================================================================
#
# ydlidar_strip_optimization
#
function(ydlidar_strip_optimization name)
set(_compile_flags)
separate_arguments(_args UNIX_COMMAND ${ARGN})
foreach(_flag ${_args})
if(NOT "${_flag}" MATCHES "^-O")
set(_compile_flags "${_compile_flags} ${_flag}")
endif()
endforeach()
string(STRIP "${_compile_flags}" _compile_flags)
set(${name} "${_compile_flags}" PARENT_SCOPE)
endfunction()
#=============================================================================
#
# ydlidar_join
#
# This function joins a list with a given separator. If list is not
# passed, or is sent "", this will return the empty string.
#
# Usage:
# ydlidar_join(OUT ${OUT} [ LIST ${LIST} ] GLUE ${GLUE})
#
# Input:
# LIST : list to join
# GLUE : separator to use
#
# Output:
# OUT : joined list
#
# Example:
# ydlidar_join(OUT test_join LIST a b c GLUE ";")
# test_join would then be:
# "a;b;c"
#
function(ydlidar_join)
ydlidar_parse_function_args(
NAME ydlidar_join
ONE_VALUE OUT GLUE
MULTI_VALUE LIST
REQUIRED GLUE OUT
ARGN ${ARGN})
string (REPLACE ";" "${GLUE}" _TMP_STR "${LIST}")
set(${OUT} ${_TMP_STR} PARENT_SCOPE)
endfunction()
#=============================================================================
function(ydlidar_add_module)
ydlidar_parse_function_args(
NAME ydlidar_add_module
ONE_VALUE MODULE
MULTI_VALUE COMPILE_FLAGS LINK_FLAGS SRCS INCLUDES DEPENDS
OPTIONS EXTERNAL
REQUIRED MODULE
ARGN ${ARGN})
ydlidar_add_library(${MODULE} STATIC EXCLUDE_FROM_ALL ${SRCS})
# Pass variable to the parent ydlidar_add_module.
set(_no_optimization_for_target ${_no_optimization_for_target} PARENT_SCOPE)
if(COMPILE_FLAGS)
target_compile_options(${MODULE} PRIVATE ${COMPILE_FLAGS})
endif()
if(INCLUDES)
target_include_directories(${MODULE} PRIVATE ${INCLUDES})
endif()
if(DEPENDS)
add_dependencies(${MODULE} ${DEPENDS})
endif()
# join list variables to get ready to send to compiler
foreach(prop LINK_FLAGS)
if(${prop})
ydlidar_join(OUT ${prop} LIST ${${prop}} GLUE " ")
endif()
endforeach()
if(COMPILE_FLAGS AND ${_no_optimization_for_target})
ydlidar_strip_optimization(COMPILE_FLAGS ${COMPILE_FLAGS})
endif()
foreach (prop LINK_FLAGS )
if (${prop})
set_target_properties(${MODULE} PROPERTIES ${prop} ${${prop}})
endif()
endforeach()
set_property(GLOBAL APPEND PROPERTY YDLIDAR_LIBRARIES ${MODULE})
set_property(GLOBAL APPEND PROPERTY YDLIDAR_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
endfunction()
#=============================================================================
#
# ydlidar_add_optimization_flags_for_target
#
set(all_posix_cmake_targets "" CACHE INTERNAL "All cmake targets for which optimization can be suppressed")
function(ydlidar_add_optimization_flags_for_target target)
set(_no_optimization_for_target FALSE)
# If the current CONFIG is posix_sitl_* then suppress optimization for certain targets.
foreach(_regexp $ENV{CORE_NO_OPTIMIZATION})
if("${target}" MATCHES "${_regexp}")
set(_no_optimization_for_target TRUE)
set(_matched_regexp "${_regexp}")
endif()
endforeach()
# Create a full list of targets that optimization can be suppressed for.
list(APPEND all_posix_cmake_targets ${target})
set(all_posix_cmake_targets ${all_posix_cmake_targets} CACHE INTERNAL "All cmake targets for which optimization can be suppressed")
if(NOT ${_no_optimization_for_target})
target_compile_options(${target} PRIVATE ${optimization_flags})
else()
message(STATUS "Disabling optimization for target '${target}' because it matches the regexp '${_matched_regexp}' in env var CORE_NO_OPTIMIZATION")
target_compile_options(${target} PRIVATE -O0)
endif()
# Pass variable to the parent ydlidar_add_library.
set(_no_optimization_for_target ${_no_optimization_for_target} PARENT_SCOPE)
endfunction()
#=============================================================================
#
# ydlidar_add_executable
#
# Like add_executable but with optimization flag fixup.
#
function(ydlidar_add_executable target)
add_executable(${target} ${ARGN})
ydlidar_add_optimization_flags_for_target(${target})
endfunction()
#=============================================================================
#
# ydlidar_add_library
#
# Like add_library but with optimization flag fixup.
#
function(ydlidar_add_library target)
add_library(${target} ${ARGN})
ydlidar_add_optimization_flags_for_target(${target})
# Pass variable to the parent ydlidar_add_module.
set(_no_optimization_for_target ${_no_optimization_for_target} PARENT_SCOPE)
set_property(GLOBAL APPEND PROPERTY YDLIDAR_LIBRARIES ${target})
set_property(GLOBAL APPEND PROPERTY YDLIDAR_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
endfunction()
#=============================================================================
#
# ydlidar_prebuild_targets
#
# This function generates os dependent targets
#
# Usage:
# ydlidar_prebuild_targets(
# OUT <out-list_of_targets>
# BOARD <in-string>
# )
#
# Input:
# BOARD : board
#
# Output:
# OUT : the target list
#
# Example:
# ydlidar_prebuild_targets(OUT target_list BOARD r16)
#
function(ydlidar_prebuild_targets)
ydlidar_parse_function_args(
NAME ydlidar_prebuild_targets
ONE_VALUE OUT BOARD
REQUIRED OUT
ARGN ${ARGN})
add_library(prebuild_targets INTERFACE)
#add_dependencies(prebuild_targets DEPENDS)
endfunction()

71
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_parse.cmake Normal file
View File

@@ -0,0 +1,71 @@
include(common/ydlidar_base)
#############################################################################
# Setup libraries
get_property( INTERNAL_INC GLOBAL PROPERTY YDLIDAR_INCLUDE_DIRS )
get_property( INTERNAL_LIBS GLOBAL PROPERTY YDLIDAR_LIBRARIES )
get_property( SDK_SOURCES GLOBAL PROPERTY YDLIDAR_SOURCES )
get_property( SDK_HEADERS GLOBAL PROPERTY YDLIDAR_HEADERS )
# this is a horrible hack in order to set compiler flag properties to individual files
get_property( C_O_S GLOBAL PROPERTY COMPILER_OPTS_SOURCES )
get_property( C_O_F GLOBAL PROPERTY COMPILER_OPTS_FLAGS )
list(LENGTH C_O_S len_c_o_s )
math(EXPR len_c_o_s "${len_c_o_s} - 1" )
foreach(val RANGE ${len_c_o_s} )
list(GET C_O_S ${val} source )
list(GET C_O_F ${val} flag )
set_source_files_properties( ${source} PROPERTIES COMPILE_FLAGS ${flag} )
endforeach()
##########################################################################
include_directories(${CMAKE_SOURCE_DIR} ${CMAKE_BINARY_DIR} )
include_directories( ${LIB_INC_DIR} )
include_directories( ${INTERNAL_INC} )
##########################################################################
# ccache: a compiler, can speed up gcc compilation.
#
option(CCACHE "Use ccache if available" ON)
find_program(CCACHE_PROGRAM ccache)
if (CCACHE AND CCACHE_PROGRAM AND NOT DEFINED ENV{CCACHE_DISABLE})
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "${CCACHE_PROGRAM}")
else()
endif()
##########################################################################
# generate compile command database
#
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
##########################################################################
# check required toolchain variables
#
# search for programs in the build host directories
SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
# for libraries and headers in the target directories
SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
#############################################################################
set(SDK_LIBS
${INTERNAL_LIBS}
${LINK_LIBS}
CACHE STRING "SDK required libraries"
)
list( REMOVE_ITEM SDK_LIBS "debug" )
list( REMOVE_ITEM SDK_LIBS "optimized" )
set(SDK_INCS
${INTERNAL_INC}
${USER_INC}
CACHE STRING "SDK required incs" )

296
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/install_package.cmake Normal file
View File

@@ -0,0 +1,296 @@
################################################################################
# install_package.cmake - This function will install and "export" your library
# or files in such a way that they can be found using either CMake's
# "FindXXX.cmake" mechanism or with pkg-config. This makes your code boradly
# compatible with traditional unix best practices, and also easy to use from
# other CMake projets.
#
# This function will create and install a ${PACKAGE}.pc pkg-config file.
# Will also create a Find${PACKAGE}.cmake, which will in turn call.
#
# install_package - Takes a package name and the following optional named arguments:
# PKG_NAME <name of the package for pkg-config>, usually the same as ${PROJECT_NAME}
# LIB_NAME <name of a library to build, if any>
# VERSION <version>
# INSTALL_HEADERS <header files to install, if any>
# DESTINATION <directory to install headers>
# INCLUDE_DIRS <list of required include directories, if any>
# LINK_LIBS <list of required link libraries, if any >
# LINK_DIRS <list of required link directories, if any>
# CFLAGS <optional list of REQUIRED c flags>
# CXXFLAGS <optional list of REQUIRED c++ flags>
#
################################################################################
include(CMakeParseArguments)
get_filename_component(modules_dir ${CMAKE_CURRENT_LIST_FILE} PATH)
function(install_package)
set(PACKAGE_OPTIONS "")
set(PACKAGE_SINGLE_ARGS "")
set( PACKAGE_MULTI_ARGS
PKG_NAME
LIB_NAME
VERSION
DESCRIPTION
INSTALL_HEADERS
INSTALL_GENERATED_HEADERS
INSTALL_HEADER_DIRS
INSTALL_INCLUDE_DIR
DESTINATION
INCLUDE_DIRS
LINK_LIBS
LINK_DIRS
CFLAGS
CXXFLAGS
)
cmake_parse_arguments( PACKAGE
"${PACKAGE_OPTIONS}"
"${PACKAGE_SINGLE_ARGS}"
"${PACKAGE_MULTI_ARGS}"
"${ARGN}"
)
# Add package to CMake package registery for use from the build tree. RISKY.
option( EXPORT_${PROJECT_NAME}
"Should the ${PROJECT_NAME} package be exported for use by other software" OFF )
mark_as_advanced( EXPORT_${PROJECT_NAME} )
# clean things up
if( PACKAGE_LINK_DIRS )
list( REMOVE_DUPLICATES PACKAGE_LINK_DIRS )
endif()
if(PACKAGE_LINK_LIBS)
list( REMOVE_DUPLICATES PACKAGE_LINK_LIBS )
endif()
if( PACKAGE_INCLUDE_DIRS)
list( REMOVE_DUPLICATES PACKAGE_INCLUDE_DIRS )
endif()
# construct Cflags arguments for pkg-config file
set( PACKAGE_CFLAGS "${PACKAGE_CFLAGS} ${CMAKE_C_FLAGS}" )
foreach(var IN LISTS PACKAGE_INCLUDE_DIRS )
set( PACKAGE_CFLAGS "${PACKAGE_CFLAGS} -I${var}" )
endforeach()
# now construct Libs.private arguments
foreach(var IN LISTS PACKAGE_LINK_DIRS )
set( PACKAGE_LIBS "${PACKAGE_LIBS} -L${var}" )
endforeach()
foreach(var IN LISTS PACKAGE_LINK_LIBS )
if( EXISTS ${var} OR ${var} MATCHES "-framework*" )
set( PACKAGE_LIBS "${PACKAGE_LIBS} ${var}" )
else() # assume it's just a -l call??
set( PACKAGE_LIBS "${PACKAGE_LIBS} -l${var}" )
endif()
endforeach()
# add any CXX flags user has passed in
if( PACKAGE_CXXFLAGS )
set( PACKAGE_CFLAGS ${PACKAGE_CXXFLAGS} )
endif()
# In case we want to install.
if( NOT EXPORT_${PROJECT_NAME} )
# add "installed" library to list of required libraries to link against
if( PACKAGE_LIB_NAME )
if(POLICY CMP0026)
cmake_policy( SET CMP0026 OLD )
endif()
get_target_property( _target_library ${PACKAGE_LIB_NAME} LOCATION )
get_filename_component( _lib ${_target_library} NAME )
list( INSERT PACKAGE_LINK_LIBS 0 ${PACKAGE_LIB_NAME} )
endif()
if( PACKAGE_INSTALL_HEADER_DIRS )
foreach(dir IN LISTS PACKAGE_INSTALL_HEADER_DIRS )
install( DIRECTORY ${dir}
DESTINATION ${PACKAGE_DESTINATION}/include
FILES_MATCHING PATTERN "*.h|*.hxx|*.hpp"
)
endforeach()
endif()
# install header files
if( PACKAGE_INSTALL_HEADERS )
foreach(hdr IN LISTS PACKAGE_INSTALL_HEADERS )
get_filename_component( _fp ${hdr} ABSOLUTE )
if("${SDK_SOURCE_DIR}" STREQUAL "")
file( RELATIVE_PATH _rpath ${CMAKE_SOURCE_DIR} ${_fp} )
else()
file( RELATIVE_PATH _rpath ${SDK_SOURCE_DIR} ${_fp} )
endif()
get_filename_component( _dir ${_rpath} DIRECTORY )
install( FILES ${_fp}
DESTINATION ${PACKAGE_DESTINATION}/${_dir} )
endforeach()
endif()
if( PACKAGE_INSTALL_GENERATED_HEADERS )
foreach(hdr IN LISTS PACKAGE_INSTALL_GENERATED_HEADERS )
get_filename_component( _fp ${hdr} ABSOLUTE )
file( RELATIVE_PATH _rpath ${CMAKE_BINARY_DIR} ${_fp} )
get_filename_component( _dir ${_rpath} DIRECTORY )
install( FILES ${_fp}
DESTINATION ${PACKAGE_DESTINATION}/${_dir} )
endforeach()
endif()
if( PACKAGE_INSTALL_INCLUDE_DIR )
if("${SDK_SOURCE_DIR}" STREQUAL "")
install(DIRECTORY ${CMAKE_SOURCE_DIR}/include DESTINATION ${PACKAGE_DESTINATION} )
else()
install(DIRECTORY ${SDK_SOURCE_DIR}/include DESTINATION ${PACKAGE_DESTINATION} )
endif()
endif()
# install library itself
if( PACKAGE_LIB_NAME )
install( FILES ${_target_library} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib )
set( PACKAGE_LIB_LINK "-l${PACKAGE_LIB_NAME}" )
endif()
# build pkg-config file
if( PACKAGE_PKG_NAME )
configure_file( ${modules_dir}/PkgConfig.pc.in ${PACKAGE_PKG_NAME}.pc @ONLY )
# install pkg-config file for external projects to discover this library
install( FILES ${CMAKE_CURRENT_BINARY_DIR}/${PACKAGE_PKG_NAME}.pc
DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/pkgconfig/ )
#######################################################
# Export library for easy inclusion from other cmake projects. APPEND allows
# call to function even as subdirectory of larger project.
FILE(REMOVE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake")
export( TARGETS ${LIBRARY_NAME}
APPEND FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake" )
if("${SDK_SOURCE_DIR}" STREQUAL "")
# Version information. So find_package( XXX version ) will work.
configure_file( ${CMAKE_SOURCE_DIR}/cmake/PackageConfigVersion.cmake.in
"${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" @ONLY )
# Build tree config. So some folks can use the built package (e.g., any of our
# own examples or applcations in this project.
configure_file( ${CMAKE_SOURCE_DIR}/cmake/PackageConfig.cmake.in
${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake @ONLY )
else()
# Version information. So find_package( XXX version ) will work.
configure_file( ${SDK_SOURCE_DIR}/cmake/PackageConfigVersion.cmake.in
"${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" @ONLY )
# Build tree config. So some folks can use the built package (e.g., any of our
# own examples or applcations in this project.
configure_file( ${SDK_SOURCE_DIR}/cmake/PackageConfig.cmake.in
${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake @ONLY )
endif()
install(FILES
${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake
${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake
${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake
DESTINATION
lib/cmake/${PROJECT_NAME})
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/Find${PACKAGE_PKG_NAME}.cmake
DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PACKAGE_PKG_NAME}/ )
# # Install tree config. NB we DO NOT use this. We install using brew or
# pkg-config.
# set( EXPORT_LIB_INC_DIR ${LIB_INC_DIR} )
# set( EXPORT_LIB_INC_DIR "\${PROJECT_CMAKE_DIR}/${REL_INCLUDE_DIR}" )
# configure_file( ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}Config.cmake.in
# ${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake @ONLY )
endif()
# In case we want to export.
elseif( EXPORT_${PROJECT_NAME} )
if( PACKAGE_LIB_NAME )
if(POLICY CMP0026)
cmake_policy( SET CMP0026 OLD )
endif()
get_target_property( _target_library ${PACKAGE_LIB_NAME} LOCATION )
list( INSERT PACKAGE_LINK_LIBS 0 ${_target_library} )
endif()
if( PACKAGE_INSTALL_HEADER_DIRS )
foreach(dir IN LISTS PACKAGE_INSTALL_HEADER_DIRS )
list( APPEND PACKAGE_INCLUDE_DIRS ${dir} )
endforeach()
endif()
#if( PACKAGE_INSTALL_HEADER_DIRS )
# foreach(dir IN LISTS PACKAGE_INSTALL_HEADER_DIRS )
# FILE( GLOB ${dir} "*.h" "*.hpp" )
# list( APPEND PACKAGE_INCLUDE_DIRS ${dir} )
# endforeach()
#endif()
if("${SDK_SOURCE_DIR}" STREQUAL "")
list( APPEND PACKAGE_INCLUDE_DIRS ${CMAKE_SOURCE_DIR}
${CMAKE_BINARY_DIR} )
else()
list( APPEND PACKAGE_INCLUDE_DIRS ${SDK_SOURCE_DIR}
${CMAKE_BINARY_DIR} )
endif()
# install library itself
#if( PACKAGE_LIB_NAME )
# set( PACKAGE_LIB_LINK "-l${PACKAGE_LIB_NAME}" )
#endif()
#######################################################
# Export library for easy inclusion from other cmake projects. APPEND allows
# call to function even as subdirectory of larger project.
FILE(REMOVE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake")
export( TARGETS ${LIBRARY_NAME}
APPEND FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake" )
export( PACKAGE ${PROJECT_NAME} )
# install( EXPORT ${PROJECT_NAME}Targets DESTINATION ${CMAKECONFIG_INSTALL_DIR} )
# install(TARGETS ${LIBRARY_NAME}
# EXPORT ${PROJECT_NAME}Targets DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
# )
if("${SDK_SOURCE_DIR}" STREQUAL "")
# Version information. So find_package( XXX version ) will work.
configure_file( ${CMAKE_SOURCE_DIR}/cmake/PackageConfigVersion.cmake.in
"${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" @ONLY )
# Build tree config. So some folks can use the built package (e.g., any of our
# own examples or applcations in this project.
configure_file( ${CMAKE_SOURCE_DIR}/cmake/PackageConfig.cmake.in
${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake @ONLY )
else()
# Version information. So find_package( XXX version ) will work.
configure_file( ${SDK_SOURCE_DIR}/cmake/PackageConfigVersion.cmake.in
"${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" @ONLY )
# Build tree config. So some folks can use the built package (e.g., any of our
# own examples or applcations in this project.
configure_file( ${SDK_SOURCE_DIR}/cmake/PackageConfig.cmake.in
${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake @ONLY )
endif()
# # Install tree config. NB we DO NOT use this. We install using brew or
# pkg-config.
# set( EXPORT_LIB_INC_DIR ${LIB_INC_DIR} )
# set( EXPORT_LIB_INC_DIR "\${PROJECT_CMAKE_DIR}/${REL_INCLUDE_DIR}" )
# configure_file( ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}Config.cmake.in
# ${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake @ONLY )
#export( PACKAGE ${PROJECT_NAME} )
endif()
# write and install a cmake "find package" for cmake projects to use.
# NB: this .cmake file CANNOT refer to any source directory, only to
# _installed_ files.
configure_file( ${modules_dir}/FindPackage.cmake.in Find${PACKAGE_PKG_NAME}.cmake @ONLY )
endfunction()

94
chapt9/fishbot_ws/src/ydlidar_ros2/cmake/script_show_final_summary.cmake Normal file
View File

@@ -0,0 +1,94 @@
# ----------------------------------------------------------------------------
# An auxiliary function to show messages:
# ----------------------------------------------------------------------------
MACRO(SHOW_CONFIG_LINE MSG_TEXT VALUE_BOOL)
IF(${VALUE_BOOL})
SET(VAL_TEXT "Yes")
ELSE(${VALUE_BOOL})
SET(VAL_TEXT " No")
ENDIF(${VALUE_BOOL})
MESSAGE(STATUS " ${MSG_TEXT} : ${VAL_TEXT} ${ARGV2}")
ENDMACRO(SHOW_CONFIG_LINE)
MACRO(SHOW_CONFIG_LINE_SYSTEM MSG_TEXT VALUE_BOOL)
IF(${VALUE_BOOL})
IF(${VALUE_BOOL}_SYSTEM)
SET(VAL_TEXT "Yes (System)")
ELSE(${VALUE_BOOL}_SYSTEM)
SET(VAL_TEXT "Yes (Built-in)")
ENDIF(${VALUE_BOOL}_SYSTEM)
ELSE(${VALUE_BOOL})
SET(VAL_TEXT " No")
ENDIF(${VALUE_BOOL})
MESSAGE(STATUS " ${MSG_TEXT} : ${VAL_TEXT} ${ARGV2}")
ENDMACRO(SHOW_CONFIG_LINE_SYSTEM)
# ----------------------------------------------------------------------------
# Summary:
# ----------------------------------------------------------------------------
MESSAGE(STATUS "")
IF(CMAKE_COMPILER_IS_GNUCXX)
SET(MRPT_GCC_VERSION_STR "(GCC version: ${CMAKE_MRPT_GCC_VERSION})")
ENDIF(CMAKE_COMPILER_IS_GNUCXX)
MESSAGE(STATUS "+===========================================================================+")
MESSAGE(STATUS "| Resulting configuration for ${CMAKE_MRPT_COMPLETE_NAME} |")
MESSAGE(STATUS "+===========================================================================+")
MESSAGE(STATUS " _________________________ PLATFORM _____________________________")
MESSAGE(STATUS " Host : " ${CMAKE_HOST_SYSTEM_NAME} ${CMAKE_HOST_SYSTEM_VERSION} ${CMAKE_HOST_SYSTEM_PROCESSOR})
if(CMAKE_CROSSCOMPILING)
MESSAGE(STATUS " Target : " ${CMAKE_SYSTEM_NAME} ${CMAKE_SYSTEM_VERSION} ${CMAKE_SYSTEM_PROCESSOR})
endif(CMAKE_CROSSCOMPILING)
SHOW_CONFIG_LINE("Is the system big endian? " CMAKE_MRPT_IS_BIG_ENDIAN)
MESSAGE(STATUS " Word size (32/64 bit) : ${CMAKE_MRPT_WORD_SIZE}")
MESSAGE(STATUS " CMake version : " ${CMAKE_VERSION})
MESSAGE(STATUS " CMake generator : " ${CMAKE_GENERATOR})
MESSAGE(STATUS " CMake build tool : " ${CMAKE_BUILD_TOOL})
MESSAGE(STATUS " Compiler : " ${CMAKE_CXX_COMPILER_ID})
if(MSVC)
MESSAGE(STATUS " MSVC : " ${MSVC_VERSION})
endif(MSVC)
if(CMAKE_GENERATOR MATCHES Xcode)
MESSAGE(STATUS " Xcode : " ${XCODE_VERSION})
endif(CMAKE_GENERATOR MATCHES Xcode)
if(NOT CMAKE_GENERATOR MATCHES "Xcode|Visual Studio")
MESSAGE(STATUS " Configuration : " ${CMAKE_BUILD_TYPE})
endif(NOT CMAKE_GENERATOR MATCHES "Xcode|Visual Studio")
IF("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
MESSAGE( STATUS "C++ flags (Release): ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_RELEASE}")
ELSEIF("${CMAKE_BUILD_TYPE}" STREQUAL "Debug")
MESSAGE( STATUS "C++ flags (Debug): ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_DEBUG}")
ENDIF()
MESSAGE(STATUS "")
MESSAGE(STATUS " __________________________ OPTIONS _____________________________")
SHOW_CONFIG_LINE("Build YDLidar-SDK as a shared library?　　" BUILD_SHARED_LIBS)
SHOW_CONFIG_LINE("Build Examples?　　　　　　　　　　　　　　" BUILD_EXAMPLES)
SHOW_CONFIG_LINE("Build C Sharp API?　　　　　　　　　　　　" BUILD_CSHARP)
SHOW_CONFIG_LINE("Build TEST? 　　　" BUILD_TEST)
MESSAGE(STATUS "")
MESSAGE(STATUS " _________________________ INSTALL _____________________")
MESSAGE(STATUS " Install prefix : ${CMAKE_INSTALL_PREFIX}")
MESSAGE(STATUS "")
IF($ENV{VERBOSE})
MESSAGE(STATUS " _________________________ COMPILER OPTIONS _____________________")
message(STATUS "Compiler: ${CMAKE_CXX_COMPILER} ${MRPT_GCC_VERSION_STR} ")
message(STATUS " C++ flags (Release): ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_RELEASE}")
message(STATUS " C++ flags (Debug): ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_DEBUG}")
message(STATUS " Executable link flags (Release): ${CMAKE_EXE_LINKER_FLAGS} ${CMAKE_EXE_LINKER_FLAGS_RELEASE}")
message(STATUS " Executable link flags (Debug): ${CMAKE_EXE_LINKER_FLAGS} ${CMAKE_EXE_LINKER_FLAGS_DEBUG}")
message(STATUS " Lib link flags (Release): ${CMAKE_SHARED_LINKER_FLAGS} ${CMAKE_SHARED_LINKER_FLAGS_RELEASE}")
message(STATUS " Lib link flags (Debug): ${CMAKE_SHARED_LINKER_FLAGS} ${CMAKE_SHARED_LINKER_FLAGS_DEBUG}")
MESSAGE(STATUS "")
ENDIF($ENV{VERBOSE})
MESSAGE(STATUS " _______________________ WRAPPERS/BINDINGS ______________________")
SHOW_CONFIG_LINE("Python bindings (pyydlidar) " SWIG_FOUND)
SHOW_CONFIG_LINE(" - dep: Swig found? " SWIG_FOUND "[Version: ${SWIG_VERSION}]")
SHOW_CONFIG_LINE(" - dep: PythonLibs found? " PYTHONLIBS_FOUND "[Version: ${PYTHON_VERSION_STRING}]")
MESSAGE(STATUS "")

177
chapt9/fishbot_ws/src/ydlidar_ros2/config/ydlidar.rviz Executable file
View File

@@ -0,0 +1,177 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /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: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/LaserScan
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 1012
Min Color: 0; 0; 0
Min Intensity: 1008
Name: LaserScan
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.05999999865889549
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
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_link:
Value: true
laser_frame:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
base_link:
laser_frame:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: laser_frame
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: 10
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: 1.5103975534439087
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 6.163581848144531
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: 67
Y: 60

69
chapt9/fishbot_ws/src/ydlidar_ros2/docs/paramters.md Normal file
View File

@@ -0,0 +1,69 @@
#　ROS2 Paramters Table
##　Paramters Description
| Parameter | Type | Description |
|-----------------------|------------------------|-----------------------------------------------------|
| `port` | String | port of lidar (ex. /dev/ttyUSB0) |
| `baudrate` | int | baudrate of lidar (ex. 230400) |
| `frame_id` | String | TF frame of sensor, default: `laser_frame` |
| `singleChannel` | bool | Whether LiDAR is a single-channel, default: false |
| `resolution_fixed` | bool | Fixed angluar resolution, default: true |
| `auto_reconnect` | bool | Automatically reconnect the LiDAR, default: true |
| `reversion` | bool | Reversion LiDAR, default: true |
| `isToFLidar` | bool | Whether LiDAR is TOF Type, default: false |
| `angle_min` | float | Minimum Valid Angle, defalut: -180.0 |
| `angle_max` | float | Maximum Valid Angle, defalut: 180.0 |
| `range_min` | float | Minimum Valid range, defalut: 0.01m |
| `range_max` | float | Maximum Valid range, defalut: 64.0m |
| `ignore_array` | String | LiDAR filtering angle area, default: "" |
| `samp_rate` | int | sampling rate of lidar, default: 9 |
| `frequency` | float | scan frequency of lidar,default: 10.0 |
##　Baudrate Table
| LiDAR | baudrate |
|-----------------------------------------------|-----------------------|
|F4/S2/X2/X2L/S4/TX8/TX20/G4C | 115200 |
|X4 | 128000 |
|S4B | 153600 |
|G1/G2/R2/G4/G4PRO/F4PRO | 230400 |
|G6/TG15/TG30/TG50 | 512000 |
## SingleChannel Table
| LiDAR | singleChannel |
|-----------------------------------------------------------|-----------------------|
|G1/G2/G4/G6/F4/F4PRO/S4/S4B/X4/R2/G4C | false |
|S2/X2/X2L | true |
|TG15/TG30/TG50 | false |
|TX8/TX20 | true |
##isToFLidar Table
| LiDAR | isToFLidar |
|-----------------------------------------------------------------------|-----------------------|
|G1/G2/G4/G6/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L | false |
|TG15/TG30/TG50/TX8/TX20 | true |
## Sampling Rate Table
| LiDAR | samp_rate |
|-----------------------------|------------------------|
|G4/F4 | 4,8,9 |
|F4PRO | 4,6 |
|G6 | 8,16,18 |
|G1/G2/R2/X4 | 5 |
|S4/S4B/G4C/TX8/TX20 | 4 |
|S2 | 3 |
|TG15/TG30/TG50 | 10,18,20 |
##　Frequency Table
| LiDAR | frequency |
|-----------------------------------------------|------------------------|
|G1/G2/R2/G6/G4/G4PRO/F4/F4PRO | 5-12Hz |
|S4/S4B/S2/TX8/TX20/X4 | Not Support |
|TG15/TG30/TG50 | 3-16Hz |
Note: For unsupported LiDARs, adjusting the scanning frequency requires external access to PWM speed control.

58
chapt9/fishbot_ws/src/ydlidar_ros2/docs/ydlidar.md Normal file
View File

@@ -0,0 +1,58 @@
#　YDLIDAR ROS2 Package Download and Build
##　Step1: create a ROS2 workspace, if there is no workspace, othereise Skip to Step2
####　Linux/OS X
$mkdir -p ~/ydlidar_ros2_ws/src
$cd ~/ydlidar_ros2_ws/src
####　Windows
$md \dev\ydlidar_ros2_ws\src
$cd \dev\ydlidar_ros2_ws\src
##　Step2: clone ydlidar ros2 package
$git clone https://github.com/YDLIDAR/ydlidar_ros2
##　Step3: Build [ydlidar_ros2](https://github.com/YDLIDAR/ydlidar_ros2) package
$cd ..
$colcon build --symlink-install
Note: install colcon [see](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#install-colcon)
##　Step4:Configure LiDAR [paramters](params/ydlidar.yaml)
ydlidar_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 230400
samp_rate: 9
resolution_fixed: true
singleChannel: false
auto_reconnect: true
reversion: true
isToFLidar: false
angle_max: 180.0
angle_min: -180.0
max_range: 16.0
min_range: 0.01
frequency: 10.0
Note: How to configure paramters, see [here](paramters.md)
## Step5: Create serial port Alias[/dev/ydlidar]
$chmod 0777 src/ydlidar_ros2/startup/*
$sudo sh src/ydlidar_ros2/startup/initenv.sh
Note: After completing the previous operation, replug the LiDAR again.
##　Step6:Run ydlidar_ros2 node
$ros2 run ydlidar ydlidar_node
$ros2 run ydlidar ydlidar_client
or
$ros2 launch ydlidar ydlidar_launch.py
$ros2 run ydldiar ydlidar_client
or
$ros2 topic echo /scan

29
chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar.py Normal file
View File

@@ -0,0 +1,29 @@
from launch.exit_handler import ignore_exit_handler, restart_exit_handler
from ros2run.api import get_executable_path
def launch(launch_descriptor, argv):
ld = launch_descriptor
package = 'ydlidar'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='ydlidar_node')],
name='ydlidar_node',
exit_handler=restart_exit_handler,
)
package = 'tf2_ros'
ld.add_process(
# The XYZ/Quat numbers for base_link -> laser_frame are taken from the
# turtlebot URDF in
# https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
cmd=[
get_executable_path(
package_name=package, executable_name='static_transform_publisher'),
'0', '0', '0.02',
'0', '0', '0', '1',
'base_link',
'laser_frame'
],
name='static_tf_pub_laser',
exit_handler=restart_exit_handler,
)
return ld

49
chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar_launch.py Normal file
View File

@@ -0,0 +1,49 @@
#!/usr/bin/python3
# Copyright 2020, EAIBOT
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import LogInfo
import lifecycle_msgs.msg
import os
def generate_launch_description():
share_dir = get_package_share_directory('ydlidar')
parameter_file = LaunchConfiguration('params_file')
node_name = 'ydlidar_node'
params_declare = DeclareLaunchArgument('params_file',
default_value=os.path.join(
share_dir, 'params', 'ydlidar.yaml'),
description='FPath to the ROS2 parameters file to use.')
driver_node = LifecycleNode(package='ydlidar',
executable='ydlidar_node',
name='ydlidar_node',
output='screen',
emulate_tty=True,
parameters=[parameter_file],
namespace='/',
)
return LaunchDescription([
params_declare,
driver_node,
])

31
chapt9/fishbot_ws/src/ydlidar_ros2/package.xml Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>ydlidar</name>
<version>1.4.5</version>
<description>
YDLidar LiDAR sensor nodes
</description>
<maintainer email="support@ydlidar.com">Tony</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rclcpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

24
chapt9/fishbot_ws/src/ydlidar_ros2/params/ydlidar.yaml Normal file
View File

@@ -0,0 +1,24 @@
ydlidar_node:
ros__parameters:
port: /tmp/tty_laser # 激光雷达端口
frame_id: laser_link # 坐标系ID
ignore_array: "" # 忽略的数组
baudrate: 115200 # 波特率
lidar_type: 1 # 激光雷达类型
device_type: 0 # 设备类型
sample_rate: 3 # 采样率
intensity_bit: 8 # 强度位数
abnormal_check_count: 4 # 异常检查计数
fixed_resolution: false # 固定分辨率
reversion: true # 反转
inverted: true # 倒置
auto_reconnect: true # 自动重连
isSingleChannel: true # 单通道
intensity: true # 强度
support_motor_dtr: true # 支持电机DTR
angle_max: 180.0 # 最大角度
angle_min: -180.0 # 最小角度
range_max: 64.0 # 最大范围
range_min: 0.05 # 最小范围
frequency: 5.0 # 频率
invalid_range_is_inf: false # 无效范围为无穷大

83
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/CMakeLists.txt Executable file
View File

@@ -0,0 +1,83 @@
cmake_minimum_required(VERSION 2.8)
# project(ydlidar_sdk C CXX)
#########################################################
set(YDLIDAR_SDK_VERSION_MAJOR 1)
set(YDLIDAR_SDK_VERSION_MINOR 2)
set(YDLIDAR_SDK_VERSION_PATCH 9)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
set(YDSDK_NAME "ydsdk")
##########################################################
# Detect wordsize:
IF(CMAKE_SIZEOF_VOID_P EQUAL 8) # Size in bytes!
SET(CMAKE_MRPT_WORD_SIZE 64)
ELSE()
SET(CMAKE_MRPT_WORD_SIZE 32)
ENDIF()
##################################################
# Add c++11 Flag
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
##########################################################
IF (CMAKE_SYSTEM_NAME MATCHES "Linux")
MESSAGE(STATUS "Current platform: Linux")
#Linux add -fPIC
add_compile_options(-fPIC)
ENDIF (CMAKE_SYSTEM_NAME MATCHES "Linux")
#or
#set(CMAKE_POSITION_INDEPENDENT_CODE ON)
#####################################################
# add cmake module path
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
set(SDK_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
#############################################################################
# include cmake file
include(common/ydlidar_base)
############################################################################
# include headers
include_directories(.)
include_directories(core)
include_directories(src)
#############################################################################
# addd subdirectory
add_subdirectory(core)
add_subdirectory(src)
#############################################################################
# PARSE libraries
include(common/ydlidar_parse)
include_directories(${SDK_INCS})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
########################################################
## Create configure file for inclusion in library
# configure_file("${CMAKE_CURRENT_SOURCE_DIR}/ydlidar_config.h.in"
# "${CMAKE_CURRENT_BINARY_DIR}/ydlidar_config.h" )
# set(GENERATED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/ydlidar_config.h )
#############################################################################
# SET(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR})
#############################################################################################
#shared library
# if(BUILD_SHARED_LIBS)
# ydlidar_add_library(${YDSDK_NAME} SHARED ${SDK_SOURCES} ${SDK_HEADERS} ${GENERATED_HEADERS})
# else()
# ydlidar_add_library(${PROJECT_NAME} STATIC ${SDK_SOURCES} ${SDK_HEADERS} ${GENERATED_HEADERS})
# endif()
# target_link_libraries(${YDSDK_NAME} ${SDK_LIBS})
# append path
# list(APPEND SDK_INCS ${CMAKE_INSTALL_PREFIX}/include/src
# ${CMAKE_INSTALL_PREFIX}/include)
###############################################################################
# install package
# string(TOUPPER ${YDSDK_NAME} PROJECT_PKG_NAME)

8
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/CMakeLists.txt Normal file
View File

@@ -0,0 +1,8 @@
cmake_minimum_required(VERSION 2.8)
include_directories(.)
include_directories( ${CMAKE_CURRENT_SOURCE_DIR})
subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR})
foreach(subdir ${SUBDIRS})
include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/${subdir} )
add_subdirectory(${subdir})
endforeach()

11
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/CMakeLists.txt Normal file
View File

@@ -0,0 +1,11 @@
include_directories( ${CMAKE_CURRENT_SOURCE_DIR} )
aux_include_directory(. HDRS)
aux_source_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})
IF (WIN32)
add_to_ydlidar_libraries(Winmm)
ELSE()
add_to_ydlidar_libraries(pthread)
ENDIF()

148
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/datatype.h Normal file
View File

@@ -0,0 +1,148 @@
#ifndef DATATYPE_H_
#define DATATYPE_H_
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <signal.h>
#include <cerrno>
#include <stdexcept>
#include <csignal>
#include <sys/stat.h>
#if defined(_MSC_VER)
#include <io.h>
#endif
#include "typedef.h"
#if !defined(_MSC_VER)
#include <unistd.h>
#define _itoa(value, str, radix) {sprintf(str, "%d", value);}
#endif
#define UNUSED(x) (void)x
#if !defined(_MSC_VER)
# define _access access
#endif
#define valName(val) (#val)
#define valLastName(val) \
{ \
char* strToken; \
char str[64]; \
strncpy(str, (const char*)val, sizeof(str)); \
strToken = strtok(str, "."); \
while (strToken != NULL) { \
strcpy(val, (const char*)strToken); \
strToken = strtok(NULL, "."); \
} \
}
/**
* @class dataFrame
* @brief data frame Structure.
*
* @author jzhang
*/
#define FRAME_PREAMBLE 0xFFEE
#define LIDAR_2D 0x2
#define DATA_FRAME 0x1
#define DEFAULT_INTENSITY 10
#define DSL(c, i) ((c << i) & (0xFF << i))
/*---------------------------------------------------------------------------*/
/* */
/* Type Definition Macros */
/* */
/*---------------------------------------------------------------------------*/
#ifndef __WORDSIZE
/* Assume 32 */
#define __WORDSIZE 32
#endif
#if defined(__linux__) || defined(_DARWIN)
#include <stdint.h>
typedef int SOCKET;
#endif
#if defined(_WIN32)
struct iovec {
void *iov_base;
size_t iov_len;
};
typedef int socklen_t;
#endif
#if defined(_WIN32)
#ifndef UINT8_MAX
#define UINT8_MAX (UCHAR_MAX)
#endif
#ifndef UINT16_MAX
#define UINT16_MAX (USHRT_MAX)
#endif
#ifndef UINT32_MAX
#define UINT32_MAX (ULONG_MAX)
#endif
#if __WORDSIZE == 64
#define SIZE_MAX (18446744073709551615UL)
#else
#ifndef SIZE_MAX
#define SIZE_MAX (4294967295U)
#endif
#endif
#endif
#if defined(_WIN32)
#define ssize_t size_t
#endif
#define __small_endian
#ifndef __GNUC__
#define __attribute__(x)
#endif
#ifdef _AVR_
typedef uint8_t _size_t;
#define THREAD_PROC
#elif defined (WIN64)
typedef uint64_t _size_t;
#define THREAD_PROC __stdcall
#elif defined (WIN32)
typedef uint32_t _size_t;
#define THREAD_PROC __stdcall
#elif defined (_M_X64)
typedef uint64_t _size_t;
#define THREAD_PROC __stdcall
#elif defined (__GNUC__)
typedef unsigned long _size_t;
#define THREAD_PROC
#elif defined (__ICCARM__)
typedef uint32_t _size_t;
#define THREAD_PROC
#endif
typedef int32_t result_t;
#define RESULT_OK 0
#define RESULT_TIMEOUT -1
#define RESULT_FAIL -2
#define INVALID_TIMESTAMP (0)
#define IS_OK(x) ( (x) == RESULT_OK )
#define IS_TIMEOUT(x) ( (x) == RESULT_TIMEOUT )
#define IS_FAIL(x) ( (x) == RESULT_FAIL )
#endif // DATATYPE_H_

359
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/locker.h Normal file
View File

@@ -0,0 +1,359 @@
#pragma once
#ifdef _WIN32
#include <conio.h>
#include <windows.h>
#include <process.h>
#include <tlhelp32.h>
#include <sys/utime.h>
#include <io.h>
#include <direct.h>
#else
#include <assert.h>
#include <pthread.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <errno.h>
#endif
namespace ydlidar {
namespace core {
namespace base {
class Locker {
public:
enum LOCK_STATUS {
LOCK_OK = 0,
LOCK_TIMEOUT = -1,
LOCK_FAILED = -2
};
Locker() {
#ifdef _WIN32
_lock = NULL;
#endif
init();
}
~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;
}
#if !defined(__ANDROID__)
switch (pthread_mutex_timedlock(&_lock, &wait_time)) {
case 0:
return LOCK_OK;
case ETIMEDOUT:
return LOCK_TIMEOUT;
}
#else
struct timeval timenow;
struct timespec sleepytime;
/* This is just to avoid a completely busy wait */
sleepytime.tv_sec = 0;
sleepytime.tv_nsec = 10000000; /* 10ms */
while (pthread_mutex_trylock(&_lock) == EBUSY) {
gettimeofday(&timenow, NULL);
if (timenow.tv_sec >= wait_time.tv_sec &&
(timenow.tv_usec * 1000) >= wait_time.tv_nsec) {
return LOCK_TIMEOUT;
}
nanosleep(&sleepytime, NULL);
}
return LOCK_OK;
#endif
}
#endif
#endif
return LOCK_FAILED;
}
void unlock() {
#ifdef _WIN32
ReleaseMutex(_lock);
#else
pthread_mutex_unlock(&_lock);
#endif
}
#ifdef _WIN32
HANDLE getLockHandle() {
return _lock;
}
#else
pthread_mutex_t *getLockHandle() {
return &_lock;
}
#endif
protected:
void init() {
#ifdef _WIN32
_lock = CreateMutex(NULL, FALSE, NULL);
#else
pthread_mutex_init(&_lock, NULL);
#endif
}
void release() {
unlock();
#ifdef _WIN32
if (_lock) {
CloseHandle(_lock);
}
_lock = NULL;
#else
pthread_mutex_destroy(&_lock);
#endif
}
#ifdef _WIN32
HANDLE _lock;
#else
pthread_mutex_t _lock;
#endif
};
class Event {
public:
enum {
EVENT_OK = 1,
EVENT_TIMEOUT = 2,
EVENT_FAILED = 0,
};
explicit 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
int ret = pthread_condattr_init(&_cond_cattr);
if (ret != 0) {
fprintf(stderr, "Failed to init condattr...\n");
fflush(stderr);
}
ret = pthread_condattr_setclock(&_cond_cattr, CLOCK_MONOTONIC);
pthread_mutex_init(&_cond_locker, NULL);
ret = pthread_cond_init(&_cond_var, &_cond_cattr);
#endif
}
~ Event() {
release();
}
void set(bool isSignal = true)
{
if (isSignal) {
#ifdef _WIN32
SetEvent(_event);
#else
pthread_mutex_lock(&_cond_locker);
if (!_is_signalled)
{
_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 {
struct 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;
}
switch (pthread_cond_timedwait(&_cond_var, &_cond_locker, &wait_time))
{
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_condattr_destroy(&_cond_cattr);
pthread_mutex_destroy(&_cond_locker);
pthread_cond_destroy(&_cond_var);
#endif
}
#ifdef _WIN32
HANDLE _event;
#else
pthread_condattr_t _cond_cattr;
pthread_cond_t _cond_var;
pthread_mutex_t _cond_locker;
bool _is_signalled;
bool _isAutoReset;
#endif
};
class ScopedLocker {
public :
explicit ScopedLocker(Locker &l): _binded(l) {
_binded.lock();
}
void forceUnlock() {
_binded.unlock();
}
~ScopedLocker() {
_binded.unlock();
}
Locker &_binded;
};
}//base
}//core
}//ydlidar

179
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/thread.h Normal file
View File

@@ -0,0 +1,179 @@
#pragma once
#include "v8stdint.h"
#include "timer.h"
#ifdef _WIN32
#include <conio.h>
#include <windows.h>
#include <io.h>
#include <process.h>
#else
#include <pthread.h>
#include <assert.h>
#endif
#if defined(__ANDROID__)
#define pthread_cancel(x) 0
#endif
#define CLASS_THREAD(c, x) Thread::ThreadCreateObjectFunctor<c, &c::x>(this)
namespace ydlidar
{
namespace core
{
namespace base
{
class Thread
{
public:
template <class CLASS, int (CLASS::*PROC)(void)>
static Thread
ThreadCreateObjectFunctor(CLASS *pthis)
{
return createThread(createThreadAux<CLASS, PROC>, pthis);
}
template <class CLASS, int (CLASS::*PROC)(void)>
static _size_t THREAD_PROC
createThreadAux(void *param)
{
return (static_cast<CLASS *>(param)->*PROC)();
}
static Thread createThread(thread_proc_t proc, void *param = NULL)
{
Thread thread_(proc, param);
#if defined(_WIN32)
thread_._handle = (_size_t)(_beginthreadex(NULL, 0,
(unsigned int(__stdcall *)(void *))proc, param, 0, NULL));
#else
assert(sizeof(thread_._handle) >= sizeof(pthread_t));
int ret = pthread_create((pthread_t *)&thread_._handle,
NULL, (void *(*)(void *))proc,
param);
if (ret != 0)
{
thread_._handle = 0;
fprintf(stderr, "[YDLIDAR] Fail to create thread!\n\tError[%s]\n",
strerror(ret));
}
#endif
return thread_;
}
public:
explicit Thread() : _param(NULL), _func(NULL), _handle(0) {}
virtual ~Thread() {}
_size_t getHandle()
{
return _handle;
}
int terminate()
{
#if defined(_WIN32)
if (!this->_handle)
{
return 0;
}
if (TerminateThread(reinterpret_cast<HANDLE>(this->_handle), -1))
{
CloseHandle(reinterpret_cast<HANDLE>(this->_handle));
this->_handle = NULL;
return 0;
}
else
{
return -2;
}
#else
if (!this->_handle)
{
return 0;
}
return pthread_cancel((pthread_t)this->_handle);
#endif
}
void *getParam()
{
return _param;
}
//等待线程退出
int join(unsigned long timeout = -1)
{
if (!_handle)
return 0;
#if defined(_WIN32)
switch (WaitForSingleObject(reinterpret_cast<HANDLE>(this->_handle), timeout))
{
case WAIT_OBJECT_0:
CloseHandle(reinterpret_cast<HANDLE>(this->_handle));
this->_handle = NULL;
return 0;
case WAIT_ABANDONED:
return -2;
case WAIT_TIMEOUT:
return -1;
}
#else
UNUSED(timeout);
int s = -1;
uint32_t t = getms();
s = pthread_cancel((pthread_t)(_handle));
if (s != 0)
{
// return s;
}
printf("[YDLIDAR DEBUG] Thread [0x%X] ready to cancel[%d]\n", _handle, s);
s = pthread_join((pthread_t)(_handle), NULL);
printf("[YDLIDAR DEBUG] Thread [0x%X] ready to cancel[%d] time[%u]\n",
_handle, s, getms() - t);
if (ESRCH == s)
{
printf("[YDLIDAR] Thread [0x%X] has been canceled in other thread\n", _handle);
return s;
}
if (s != 0)
{
fprintf(stderr, "[YDLIDAR] An error occurred while thread[0x%X] cancelled!\n", _handle);
return s;
}
printf("[YDLIDAR] Thread [0x%X] has been canceled\n", _handle);
_handle = 0;
#endif
return 0;
}
//判断是否需要退出线程（限子线程内调用）
static void needExit()
{
#if defined(_WIN32)
#else
pthread_testcancel();
#endif
}
bool operator==(const Thread &right)
{
return this->_handle == right._handle;
}
protected:
explicit Thread(thread_proc_t proc, void *param) : _param(param), _func(proc),
_handle(0) {}
void *_param;
thread_proc_t _func;
_size_t _handle;
};
} // base
} // core
} // ydlidar

57
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.cpp Normal file
View File

@@ -0,0 +1,57 @@
#include "timer.h"
#if defined(_WIN32)
#include <mmsystem.h>
#pragma comment(lib, "Winmm.lib")
namespace impl {
static LARGE_INTEGER _current_freq;
void HPtimer_reset() {
BOOL ans = QueryPerformanceFrequency(&_current_freq);
_current_freq.QuadPart /= 1000;
}
uint32_t getHDTimer() {
LARGE_INTEGER current;
QueryPerformanceCounter(&current);
return (uint32_t)(current.QuadPart / (_current_freq.QuadPart));
}
uint64_t getCurrentTime() {
FILETIME t;
GetSystemTimeAsFileTime(&t);
return ((((uint64_t)t.dwHighDateTime) << 32) | ((uint64_t)t.dwLowDateTime)) *
100;
}
BEGIN_STATIC_CODE(timer_cailb) {
HPtimer_reset();
} END_STATIC_CODE(timer_cailb)
}
#else
namespace impl {
uint32_t getHDTimer() {
struct timespec t;
t.tv_sec = t.tv_nsec = 0;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec * 1000L + t.tv_nsec / 1000000L;
}
uint64_t getCurrentTime() {
#if HAS_CLOCK_GETTIME
struct timespec tim;
clock_gettime(CLOCK_REALTIME, &tim);
return static_cast<uint64_t>(tim.tv_sec) * 1000000000LL + tim.tv_nsec;
#else
struct timeval timeofday;
gettimeofday(&timeofday, NULL);
return static_cast<uint64_t>(timeofday.tv_sec) * 1000000000LL +
static_cast<uint64_t>(timeofday.tv_usec) * 1000LL;
#endif
}
}
#endif

52
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.h Normal file
View File

@@ -0,0 +1,52 @@
#pragma once
#include "v8stdint.h"
#include <assert.h>
#include <time.h>
#include <inttypes.h>
#include "utils.h"
#define BEGIN_STATIC_CODE( _blockname_ ) \
static class _static_code_##_blockname_ { \
public: \
_static_code_##_blockname_ ()
#define END_STATIC_CODE( _blockname_ ) \
} _instance_##_blockname_;
#if defined(_WIN32)
#include <windows.h>
#define delay(x) ::Sleep(x)
#else
#include <sys/time.h>
#include <unistd.h>
static inline void delay(uint32_t ms) {
while (ms >= 1000) {
usleep(1000 * 1000);
ms -= 1000;
};
if (ms != 0) {
usleep(ms * 1000);
}
}
#endif
namespace impl {
#if defined(_WIN32)
void HPtimer_reset();
#endif
YDLIDAR_API uint32_t getHDTimer();
YDLIDAR_API uint64_t getCurrentTime();
} // namespace impl
#define getms() impl::getHDTimer()
#define getTime() impl::getCurrentTime()

17
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/typedef.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef TYPEDEF_H_
#define TYPEDEF_H_
#include <stdlib.h>
#if defined(_WIN32) && !defined(__MINGW32__)
typedef signed char int8_t;
typedef unsigned char uint8_t;
typedef short int16_t;
typedef unsigned short uint16_t;
typedef int int32_t;
typedef unsigned int uint32_t;
typedef __int64 int64_t;
typedef unsigned __int64 uint64_t;
#else
#include <stdint.h>
#endif
#endif // TYPEDEF_H_

18
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/utils.h Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#ifdef WIN32
#ifdef ydlidar_IMPORTS
#define YDLIDAR_API __declspec(dllimport)
#else
#ifdef ydlidarStatic_IMPORTS
#define YDLIDAR_API
#else
#define YDLIDAR_API __declspec(dllexport)
#endif // YDLIDAR_STATIC_EXPORTS
#endif
#else
#define YDLIDAR_API
#endif // ifdef WIN32

144
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/v8stdint.h Normal file
View File

@@ -0,0 +1,144 @@
#ifndef V8STDINT_H_
#define V8STDINT_H_
#include "datatype.h"
typedef _size_t (THREAD_PROC *thread_proc_t)(void *);
//Socket
#ifndef TRUE
#define TRUE 1
#endif
#ifndef FALSE
#define FALSE 0
#endif
#ifndef htonll
#ifdef _BIG_ENDIAN
#define htonll(x) (x)
#define ntohll(x) (x)
#else
#define htonll(x) ((((uint64)htonl(x)) << 32) + htonl(x >> 32))
#define ntohll(x) ((((uint64)ntohl(x)) << 32) + ntohl(x >> 32))
#endif
#endif
/*---------------------------------------------------------------------------*/
/* */
/* Socket Macros */
/* */
/*---------------------------------------------------------------------------*/
#if defined(_WIN32)
#define SHUT_RD 0
#define SHUT_WR 1
#define SHUT_RDWR 2
#define ACCEPT(a,b,c) accept(a,b,c)
#define CONNECT(a,b,c) connect(a,b,c)
#define CLOSE(a) closesocket(a)
#define READ(a,b,c) read(a,b,c)
#define RECV(a,b,c,d) recv(a, (char *)b, c, d)
#define RECVFROM(a,b,c,d,e,f) recvfrom(a, (char *)b, c, d, (sockaddr *)e, (int *)f)
#define RECV_FLAGS MSG_WAITALL
#define SELECT(a,b,c,d,e) select((int32_t)a,b,c,d,e)
#define SEND(a,b,c,d) send(a, (const char *)b, (int)c, d)
#define SENDTO(a,b,c,d,e,f) sendto(a, (const char *)b, (int)c, d, e, f)
#define SEND_FLAGS 0
#define SENDFILE(a,b,c,d) sendfile(a, b, c, d)
#define SET_SOCKET_ERROR(x,y) errno=y
#define SOCKET_ERROR_INTERUPT EINTR
#define SOCKET_ERROR_TIMEDOUT EAGAIN
#define WRITE(a,b,c) write(a,b,c)
#define WRITEV(a,b,c) Writev(b, c)
#define GETSOCKOPT(a,b,c,d,e) getsockopt(a,b,c,(char *)d, (int *)e)
#define SETSOCKOPT(a,b,c,d,e) setsockopt(a,b,c,(char *)d, (int)e)
#define GETHOSTBYNAME(a) gethostbyname(a)
#define IOCTLSOCKET(a, b, c) ioctlsocket(a,b,(u_long*)c)
#endif
#if defined(__linux__) || defined(_DARWIN)
#define ACCEPT(a,b,c) accept(a,b,c)
#define CONNECT(a,b,c) connect(a,b,c)
#define CLOSE(a) close(a)
#define READ(a,b,c) read(a,b,c)
#define RECV(a,b,c,d) recv(a, (void *)b, c, d)
#define RECVFROM(a,b,c,d,e,f) recvfrom(a, (char *)b, c, d, (sockaddr *)e, f)
#define RECV_FLAGS MSG_WAITALL
#define SELECT(a,b,c,d,e) select(a,b,c,d,e)
#define SEND(a,b,c,d) send(a, (const int8_t *)b, c, d)
#define SENDTO(a,b,c,d,e,f) sendto(a, (const int8_t *)b, c, d, e, f)
#define SEND_FLAGS 0
#define SENDFILE(a,b,c,d) sendfile(a, b, c, d)
#define SET_SOCKET_ERROR(x,y) errno=y
#define SOCKET_ERROR_INTERUPT EINTR
#define SOCKET_ERROR_TIMEDOUT EAGAIN
#define WRITE(a,b,c) write(a,b,c)
#define WRITEV(a,b,c) writev(a, b, c)
#define GETSOCKOPT(a,b,c,d,e) getsockopt((int)a,(int)b,(int)c,(void *)d,(socklen_t *)e)
#define SETSOCKOPT(a,b,c,d,e) setsockopt((int)a,(int)b,(int)c,(const void *)d,(int)e)
#define GETHOSTBYNAME(a) gethostbyname(a)
#define IOCTLSOCKET(a, b, c) ioctl(a,b,c)
#endif
/*---------------------------------------------------------------------------*/
/* */
/* File Macros */
/* */
/*---------------------------------------------------------------------------*/
#define STRUCT_STAT struct stat
#define LSTAT(x,y) lstat(x,y)
#define FILE_HANDLE FILE *
#define CLEARERR(x) clearerr(x)
#define FCLOSE(x) fclose(x)
#define FEOF(x) feof(x)
#define FERROR(x) ferror(x)
#define FFLUSH(x) fflush(x)
#define FILENO(s) fileno(s)
#define FOPEN(x,y) fopen(x, y)
//#define FREAD(a,b,c,d) fread(a, b, c, d)
#define FSTAT(s, st) fstat(FILENO(s), st)
//#define FWRITE(a,b,c,d) fwrite(a, b, c, d)
#define STAT_BLK_SIZE(x) ((x).st_blksize)
#define DEFAULT_CONNECTION_TIMEOUT_SEC 2
#define DEFAULT_CONNECTION_TIMEOUT_USEC 800000
#define DEFAULT_REV_TIMEOUT_SEC 2
#define DEFAULT_REV_TIMEOUT_USEC 800000
/*---------------------------------------------------------------------------*/
/* */
/* Misc Macros */
/* */
/*---------------------------------------------------------------------------*/
#if defined(_WIN32)
#define STRTOULL(x) _atoi64(x)
#else
#define STRTOULL(x) strtoull(x, NULL, 10)
#endif
#if defined(_WIN32)
#define SNPRINTF _snprintf
#define PRINTF printf
#define VPRINTF vprintf
#define FPRINTF fprintf
#else
#define SNPRINTF snprintf
#define PRINTF printf
#define VPRINTF vprintf
#define FPRINTF fprintf
#endif
#define MILLISECONDS_CONVERSION 1000
#define MICROSECONDS_CONVERSION 1000000
#define NANOECONDS_CONVERSION 1000000000
#include "ydlidar.h"
#endif // V8STDINT_H_

194
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/ydlidar.h Normal file
View File

@@ -0,0 +1,194 @@
#ifndef YDLIDAR_H_
#define YDLIDAR_H_
#include "datatype.h"
#include <signal.h>
/// PropertyBuilderByName Used to generate class member variables
/// and generate set and get methods
/// type Variable type
/// access_permission Variable access(public, priavte, protected)
#define PropertyBuilderByName(type, name, access_permission)\
access_permission:\
type m_##name;\
public:\
inline void set##name(type v) {\
m_##name = v;\
}\
inline type get##name() const {\
return m_##name;\
}\
// Determine if sigaction is available
#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
#define HAS_SIGACTION
#endif
static volatile sig_atomic_t g_signal_status = 0;
#ifdef HAS_SIGACTION
static struct sigaction old_action;
#else
typedef void (* signal_handler_t)(int);
static signal_handler_t old_signal_handler = 0;
#endif
#ifdef HAS_SIGACTION
inline struct sigaction
set_sigaction(int signal_value, const struct sigaction &action)
#else
inline signal_handler_t
set_signal_handler(int signal_value, signal_handler_t signal_handler)
#endif
{
#ifdef HAS_SIGACTION
struct sigaction old_action;
ssize_t ret = sigaction(signal_value, &action, &old_action);
if (ret == -1)
#else
signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler);
// NOLINTNEXTLINE(readability/braces)
if (old_signal_handler == SIG_ERR)
#endif
{
const size_t error_length = 1024;
// NOLINTNEXTLINE(runtime/arrays)
char error_string[error_length];
#ifndef _WIN32
#if (defined(_GNU_SOURCE) && !defined(ANDROID) &&(_POSIX_C_SOURCE >= 200112L))
char *msg = strerror_r(errno, error_string, error_length);
if (msg != error_string) {
strncpy(error_string, msg, error_length);
msg[error_length - 1] = '\0';
}
#else
int error_status = strerror_r(errno, error_string, error_length);
if (error_status != 0) {
throw std::runtime_error("Failed to get error string for errno: " +
std::to_string(errno));
}
#endif
#else
strerror_s(error_string, error_length, errno);
#endif
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
error_string);
// *INDENT-ON*
}
#ifdef HAS_SIGACTION
return old_action;
#else
return old_signal_handler;
#endif
}
inline void trigger_interrupt_guard_condition(int signal_value) {
g_signal_status = signal_value;
signal(signal_value, SIG_DFL);
}
inline void
#ifdef HAS_SIGACTION
signal_handler(int signal_value, siginfo_t *siginfo, void *context)
#else
signal_handler(int signal_value)
#endif
{
// TODO(wjwwood): remove? move to console logging at some point?
printf("signal_handler(%d)\n", signal_value);
#ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO) {
if (old_action.sa_sigaction != NULL) {
old_action.sa_sigaction(signal_value, siginfo, context);
}
} else {
if (
old_action.sa_handler != NULL && // Is set
old_action.sa_handler != SIG_DFL && // Is not default
old_action.sa_handler != SIG_IGN) { // Is not ignored
old_action.sa_handler(signal_value);
}
}
#else
if (old_signal_handler) {
old_signal_handler(signal_value);
}
#endif
trigger_interrupt_guard_condition(signal_value);
}
namespace ydlidar {
namespace core {
namespace base {
/**
* @brief initialize system state
* @param argc
* @param argv
*/
inline void init() {
#ifdef HAS_SIGACTION
struct sigaction action;
memset(&action, 0, sizeof(action));
sigemptyset(&action.sa_mask);
action.sa_sigaction = ::signal_handler;
action.sa_flags = SA_SIGINFO;
::old_action = set_sigaction(SIGINT, action);
set_sigaction(SIGTERM, action);
#else
::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler);
// Register an on_shutdown hook to restore the old signal handler.
#endif
}
/**
* @brief ok
* @return
*/
inline bool ok() {
return g_signal_status == 0;
}
/**
* @brief shutdown
*/
inline void shutdown() {
trigger_interrupt_guard_condition(SIGINT);
}
/**
* @brief fileExists
* @param filename
* @return
*/
inline bool fileExists(const std::string &filename) {
#ifdef _WIN32
struct _stat info = {0};
int ret = _stat(filename.c_str(), &info);
#else
struct stat info = {0};
int ret = stat(filename.c_str(), &info);
#endif
return (ret == 0);
}
}//base
}//core
}// namespace ydlidar
#endif // YDLIDAR_H_

5
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/CMakeLists.txt Normal file
View File

@@ -0,0 +1,5 @@
aux_include_directory(. HDRS)
aux_source_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})

134
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ChannelDevice.h Normal file
View File

@@ -0,0 +1,134 @@
#pragma once
#include <core/base/v8stdint.h>
namespace ydlidar {
namespace core {
namespace common {
class ChannelDevice {
public:
ChannelDevice() {}
virtual ~ChannelDevice() {}
/**
* @brief bind device port
* @return true if successfully bind, otherwise false
*/
virtual bool bindport(const char *, uint32_t) {
return true;
}
/**
* @brief open device
* @return true if successfully open, otherwise false
*/
virtual bool open() = 0;
/**
* @brief Whether is open
* @return true if already open, otherwise false
*/
virtual bool isOpen() = 0;
/**
* @brief close serial port or network
*/
virtual void closePort() = 0;
/**
* @brief Return the number of characters in the buffer.
* @return
*/
virtual size_t available() = 0;
/*!
* @brief Flush the input and output buffers
*/
virtual void flush() = 0;
/**
* @brief Block until there is serial or network data to read or read_timeout_constant
* number of milliseconds have elapsed. The return value is greater than zero when
* the function exits with the serial port or network buffer is greater than or
* equal to data_count, false otherwise(due to timeout or select interruption).
* @param data_count A size_t that indicates how many bytes should be wait from
* the given serial port or network buffer.
* @param timeout waiting timeout time
* @param returned_size if it is not NULL, the actual number of bytes will be returned.
* @return A size_t representing the number of bytes wait as a result of the
* call to wait.
*/
virtual int waitfordata(size_t data_count, uint32_t timeout = -1,
size_t *returned_size = NULL) = 0;
/*! Read a given amount of bytes from the serial port or network and return a string
* containing the data.
*
* \param size A size_t defining how many bytes to be read.
*
* \return A std::string containing the data read from the port.
*
*/
virtual std::string readSize(size_t size = 1) = 0;
/*! Write a string to the serial port or network.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \param size A size_t that indicates how many bytes should be written from
* the given data buffer.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*/
virtual size_t writeData(const uint8_t *data, size_t size) = 0;
/*! Read a given amount of bytes from the serial port or network into a given buffer.
*
* The read function will return in one of three cases:
* * The number of requested bytes was read.
* * In this case the number of bytes requested will match the size_t
* returned by read.
* * A timeout occurred, in this case the number of bytes read will not
* match the amount requested, but no exception will be thrown. One of
* two possible timeouts occurred:
* * The inter byte timeout expired, this means that number of
* milliseconds elapsed between receiving bytes from the serial port
* exceeded the inter byte timeout.
* * The total timeout expired, which is calculated by multiplying the
* read timeout multiplier by the number of requested bytes and then
* added to the read timeout constant. If that total number of
* milliseconds elapses after the initial call to read a timeout will
* occur.
* * An exception occurred, in this case an actual exception will be thrown.
*
* \param buffer An uint8_t array of at least the requested size.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
*/
virtual size_t readData(uint8_t *data, size_t size) = 0;
/*!
* @brief Set the DTR handshaking line to the given level.
* @param level Defaults to true.
*/
virtual bool setDTR(bool level = true) {
return true;
}
/*!
* @brief Returns the singal byte time.
* @return one byte transfer time
*/
virtual int getByteTime() {
return 0;
}
/**
* @brief Returns a human-readable description of the given error code
* or the last error code of a socket or serial port
* @return error information
*/
virtual const char *DescribeError() {
return "";
}
};
}//common
}//core
}//ydlidar

644
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/DriverInterface.h Normal file
View File

@@ -0,0 +1,644 @@
#pragma once
#include <map>
#include <thread>
#include <core/base/v8stdint.h>
#include <core/base/thread.h>
#include <core/base/locker.h>
#include <core/base/datatype.h>
#include "ydlidar_protocol.h"
#include "ydlidar_def.h"
#include "ydlidar_config.h"
namespace ydlidar
{
namespace core
{
using namespace base;
namespace common
{
class DriverInterface
{
public:
// 是否单通
PropertyBuilderByName(bool, SingleChannel, protected);
// 雷达类型
PropertyBuilderByName(int, LidarType, protected);
// 设备类型（串口或网络）
PropertyBuilderByName(uint8_t, DeviceType, protected);
/**
* @brief Set and Get Sampling interval.
* @note Negative correlation between sampling interval and lidar sampling rate.\n
* sampling interval = 1e9 / sampling rate(/s)\n
* Set the LiDAR sampling interval to match the LiDAR.
* @see DriverInterface::setPointTime and DriverInterface::getPointTime
*/
PropertyBuilderByName(uint32_t, PointTime, protected);
// 是否支持DTR启动
PropertyBuilderByName(bool, SupportMotorDtrCtrl, protected);
// 是否支持心跳
PropertyBuilderByName(bool, HeartBeat, protected);
// 是否开启调试
PropertyBuilderByName(bool, Debug, protected);
// 扫描频率
PropertyBuilderByName(float, ScanFreq, protected);
// 采样率
PropertyBuilderByName(float, SampleRate, protected);
// 是否底板优先
PropertyBuilderByName(bool, Bottom, protected);
// 是否已获取到设备信息
PropertyBuilderByName(int, HasDeviceInfo, protected);
//底板设备信息
PropertyBuilderByName(device_info, BaseDevInfo, protected);
//模组设备信息
PropertyBuilderByName(device_info, ModuleDevInfo, protected);
//OTA文件
PropertyBuilderByName(std::string, OtaName, protected);
//OTA文件加密
PropertyBuilderByName(bool, OtaEncode, protected);
//是否启用自动强度判断
PropertyBuilderByName(bool, AutoIntensity, protected);
/**
* @par Constructor
*
*/
DriverInterface() : m_port(""),
m_baudrate(8000),
m_intensities(false),
m_intensityBit(10),
scan_node_buf(NULL),
scan_node_count(0),
nodeIndex(0),
retryCount(0),
isAutoReconnect(true),
isAutoconnting(false)
{
m_SingleChannel = false;
m_LidarType = TYPE_TRIANGLE;
m_DeviceType = YDLIDAR_TYPE_SERIAL;
m_PointTime = 0;
m_SupportMotorDtrCtrl = true;
m_HeartBeat = false;
m_isScanning = false;
m_isConnected = false;
m_config.motor_rpm = 1200;
m_config.laserScanFrequency = 50;
m_config.correction_angle = 20640;
m_config.correction_distance = 6144;
m_driverErrno = NoError;
m_InvalidNodeCount = 0;
m_BufferSize = 0;
m_Debug = false;
m_ScanFreq = 0;
m_Bottom = true;
m_HasDeviceInfo = EPT_None;
m_AutoIntensity = true;
}
virtual ~DriverInterface() {}
/**
* @brief Connecting Lidar \n
* After the connection if successful, you must use ::disconnect to close
* @param[in] port_path serial port
* @param[in] baudrate serial baudrate，YDLIDAR-SS：
* 230400 G2-SS-1 R2-SS-1
* @return connection status
* @retval 0 success
* @retval < 0 failed
* @note After the connection if successful, you must use ::disconnect to close
* @see function ::YDlidarDriver::disconnect ()
*/
virtual result_t connect(const char *port_path, uint32_t baudrate = 8000) = 0;
/**
* @brief Returns a human-readable description of the given error code
* or the last error code of a socket or serial port
* @param isTCP TCP or UDP
* @return error information
*/
virtual const char *DescribeError(bool isTCP = true) = 0;
static const char *DescribeDriverError(DriverError err)
{
char const *errorString = "Unknown error";
switch (err)
{
case NoError:
errorString = ("No error");
break;
case DeviceNotFoundError:
errorString = ("Device is not found");
break;
case PermissionError:
errorString = ("Device is not permission");
break;
case UnsupportedOperationError:
errorString = ("unsupported operation");
break;
case NotOpenError:
errorString = ("Device is not open");
break;
case TimeoutError:
errorString = ("Operation timed out");
break;
case BlockError:
errorString = ("Device Block");
break;
case NotBufferError:
errorString = ("Device Failed");
break;
case TrembleError:
errorString = ("Device Tremble");
break;
case LaserFailureError:
errorString = ("Laser Failure");
break;
default:
// an empty string will be interpreted as "Unknown error"
break;
}
return errorString;
}
/*!
* @brief Disconnect the LiDAR.
*/
virtual void disconnect() = 0;
/**
* @brief Get SDK Version \n
* static function
* @return Version
*/
virtual std::string getSDKVersion() {
return YDLIDAR_SDK_VERSION_STR;
}
/**
* @brief Is the Lidar in the scan \n
* @return scanning status
* @retval true scanning
* @retval false non-scanning
*/
virtual bool isscanning() const {
return m_isScanning;
}
/**
* @brief Is it connected to the lidar \n
* @return connection status
* @retval true connected
* @retval false Non-connected
*/
virtual bool isconnected() const {
return m_isConnected;
}
/**
* @brief Is there intensity \n
* @param[in] isintensities intentsity
* true intensity
* false no intensity
*/
virtual void setIntensities(const bool &isintensities)
{
m_intensities = isintensities;
}
virtual void setIntensityBit(int bit)
{
m_intensityBit = bit;
}
/**
* @brief whether to support hot plug \n
* @param[in] enable hot plug :
* true support
* false no support
*/
virtual void setAutoReconnect(const bool &enable) {
isAutoReconnect = enable;
}
/**
* @brief Get current scan update configuration.
* @returns scanCfg structure.
*/
virtual lidarConfig getFinishedScanCfg() const
{
return m_config;
}
/**
* @brief get Health status \n
* @return result status
* @retval RESULT_OK success
* @retval RESULT_FAILE or RESULT_TIMEOUT failed
*/
virtual result_t getHealth(device_health &health,
uint32_t timeout = DEFAULT_TIMEOUT) = 0;
/**
* @brief get Device information \n
* @param[in] di Device information
* @param[in] timeout timeout
* @return result status
* @retval RESULT_OK success
* @retval RESULT_FAILE or RESULT_TIMEOUT failed
*/
virtual result_t getDeviceInfo(
device_info &di,
uint32_t timeout = DEFAULT_TIMEOUT) = 0;
// 获取级联雷达设备信息
virtual result_t getDeviceInfo(
std::vector<device_info_ex> &dis,
uint32_t timeout = DEFAULT_TIMEOUT / 2)
{
device_info di;
result_t ret = getDeviceInfo(di, timeout);
if (IS_OK(ret))
{
device_info_ex die;
memcpy(&die.di, &di, DEVICEINFOSIZE);
dis.push_back(die);
}
return ret;
}
// 获取设备信息
virtual bool getDeviceInfoEx(device_info &di, int type=EPT_Module)
{
UNUSED(di);
return false;
}
/**
* @brief Turn on scanning \n
* @param[in] force Scan mode
* @param[in] timeout timeout
* @return result status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Just turn it on once
*/
virtual result_t startScan(
bool force = false,
uint32_t timeout = DEFAULT_TIMEOUT) {
UNUSED(force);
UNUSED(timeout);
return RESULT_FAIL;
}
/**
* @brief turn off scanning \n
* @return result status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
*/
virtual result_t stop() {
return RESULT_FAIL;
}
/**
* @brief Get a circle of laser data \n
* @param[in] nodebuffer Laser data
* @param[in] count one circle of laser points
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Before starting, you must start the start the scan successfully with the ::startScan function
*/
virtual result_t grabScanData(node_info *nodebuffer, size_t &count,
uint32_t timeout = DEFAULT_TIMEOUT) = 0;
/**
* @brief Get lidar scan frequency \n
* @param[in] frequency scanning frequency
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAIL failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t getScanFrequency(scan_frequency &frequency,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief Increase the scanning frequency by 1.0 HZ \n
* @param[in] frequency scanning frequency
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t setScanFrequencyAdd(scan_frequency &frequency,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief Reduce the scanning frequency by 1.0 HZ \n
* @param[in] frequency scanning frequency
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t setScanFrequencyDis(scan_frequency &frequency,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief Increase the scanning frequency by 0.1 HZ \n
* @param[in] frequency scanning frequency
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t setScanFrequencyAddMic(scan_frequency &frequency,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief Reduce the scanning frequency by 0.1 HZ \n
* @param[in] frequency scanning frequency
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t setScanFrequencyDisMic(scan_frequency &frequency,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief Get lidar sampling frequency \n
* @param[in] frequency sampling frequency
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t getSamplingRate(sampling_rate &rate,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief Set the lidar sampling frequency \n
* @param[in] rate 　　　sampling frequency
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t setSamplingRate(sampling_rate &rate,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief get lidar zero offset angle \n
* @param[in] angle　　　 zero offset angle
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t getZeroOffsetAngle(offset_angle &angle,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief set lidar heart beat \n
* @param[in] beat heart beat status
* @param[in] timeout timeout
* @return return status
* @retval RESULT_OK success
* @retval RESULT_FAILE failed
* @note Non-scan state, perform currect operation.
*/
virtual result_t setScanHeartbeat(scan_heart_beat &beat,
uint32_t timeout = DEFAULT_TIMEOUT)
{
return RESULT_FAIL;
}
/**
* @brief setDriverError
* @param er
*/
virtual void setDriverError(const DriverError &er)
{
ScopedLocker l(_error_lock);
m_driverErrno = er;
}
/**
* @brief getDriverError
* @return
*/
virtual DriverError getDriverError()
{
ScopedLocker l(_error_lock);
return m_driverErrno;
}
/**
* @brief 设置雷达工作模式（目前只针对GS2雷达）
* @param[in] mode 雷达工作模式
* @param[in] addr 雷达地址
* @return 成功返回RESULT_OK，否则返回非RESULT_OK
*/
virtual result_t setWorkMode(int mode = 0, uint8_t addr = 0x00) {
return RESULT_FAIL;
}
/**
* @brief 解析点云数据并判断带不带强度信息（目前只针对三角雷达）
* @return 成功返回RESULT_OK，否则返回非RESULT_OK
*/
virtual result_t getIntensityFlag() {
return RESULT_OK;
}
//获取俯仰角值
virtual bool getPitchAngle(float& pitch) {
UNUSED(pitch);
return false;
}
// 开始OTA升级
virtual bool ota() {
return false;
}
public:
enum YDLIDAR_MODLES
{
YDLIDAR_None = 0,
YDLIDAR_F4 = 1, /**< F4 LiDAR Model. */
YDLIDAR_T1 = 2, /**< T1 LiDAR Model. */
YDLIDAR_F2 = 3, /**< F2 LiDAR Model. */
YDLIDAR_S4 = 4, /**< S4 LiDAR Model. */
YDLIDAR_S2PRO = YDLIDAR_S4, /**< S2PRO LiDAR Model. */
YDLIDAR_G4 = 5, /**< G4 LiDAR Model. */
YDLIDAR_X4 = 6, /**< X4 LiDAR Model. */
YDLIDAR_G4PRO = 7, /**< G4PRO LiDAR Model. */
YDLIDAR_F4PRO = 8, /**< F4PRO LiDAR Model. */
YDLIDAR_R2 = 9, /**< R2 LiDAR Model. */
YDLIDAR_G10 = 10, /**< G10 LiDAR Model. */
YDLIDAR_S4B = 11, /**< S4B LiDAR Model. */
YDLIDAR_S2 = 12, /**< S2 LiDAR Model. */
YDLIDAR_G6 = 13, /**< G6 LiDAR Model. */
YDLIDAR_G2A = 14, /**< G2A LiDAR Model. */
YDLIDAR_G2B = 15, /**< G2 LiDAR Model. */
YDLIDAR_G2C = 16, /**< G2C LiDAR Model. */
YDLIDAR_G4B = 17, /**< G4B LiDAR Model. */
YDLIDAR_G4C = 18, /**< G4C LiDAR Model. */
YDLIDAR_G1 = 19, /**< G1 LiDAR Model. */
YDLIDAR_G5 = 20, /**< G5 LiDAR Model. */
YDLIDAR_G7 = 21, /**< G7 LiDAR Model. */
YDLIDAR_SCL = 22, // SCL雷达
YDLIDAR_R3 = 23, //R3雷达
YDLIDAR_GS2 = 51, // GS2雷达
YDLIDAR_GS1 = 52, // GS1雷达
YDLIDAR_GS5 = 53, // GS5雷达
YDLIDAR_GS6 = 54, // GS6雷达
YDLIDAR_TG15 = 100, /**< TG15 LiDAR Model. */
YDLIDAR_TG30 = 101, /**< T30 LiDAR Model. */
YDLIDAR_TG50 = 102, /**< TG50 LiDAR Model. */
YDLIDAR_TEA = 110, //TEA雷达
YDLIDAR_TSA = 130, /**< TSA LiDAR Model. */
YDLIDAR_TSAPro = 131, /**< TSA Pro LiDAR Model. */
YDLIDAR_Tmini = 140, /**< Tmini LiDAR Model. */
YDLIDAR_TminiPro = 150, /**< Tmini Pro LiDAR Model. */
YDLIDAR_TminiPlus = 151, /**< Tmini Plus LiDAR Model. */
YDLIDAR_TminiPlusSH = 152, //Tmini Plus 森合
YDLIDAR_SDM15 = 160, //SDM15单点雷达
YDLIDAR_SDM18, //DTS单点雷达
YDLIDAR_T15 = 200, /**< T15 LiDAR Model. */
YDLIDAR_TIA = 210, //TIA雷达
YDLIDAR_TIA_H = 211, //TIA-H雷达
YDLIDAR_TIA_X = 212, //TIA-X雷达
YDLIDAR_Tail = 255,
};
enum YDLIDAR_RATE
{
YDLIDAR_RATE_4K = 0, /**< 4K sample rate code */
YDLIDAR_RATE_8K = 1, /**< 8K sample rate code */
YDLIDAR_RATE_9K = 2, /**< 9K sample rate code */
YDLIDAR_RATE_10K = 3, /**< 10K sample rate code */
};
public:
enum
{
DEFAULT_TIMEOUT = 2000, /**< Default timeout. */
DEFAULT_HEART_BEAT = 1000, /**< Default heartbeat timeout. */
MAX_SCAN_NODES = 5000, /**< Default Max Scan Count. */
DEFAULT_TIMEOUT_COUNT = 2, /**< Default Timeout Count. */
};
protected:
/* Variable for LIDAR compatibility */
/// LiDAR Scanning state
bool m_isScanning = false;
/// LiDAR connected state
bool m_isConnected = false;
/// Scan Data Event
Event _dataEvent;
/// Data Locker（不支持嵌套）
Locker _lock;
/// Parse Data thread
Thread _thread; //线程对象
std::thread* m_thread = nullptr; //STD线程对象
std::thread* m_thread2 = nullptr; //STD线程对象
/// command locker（不支持嵌套）
Locker _cmd_lock;
/// driver error locker（不支持嵌套）
Locker _error_lock;
/// LiDAR com port or IP Address
std::string m_port;
/// baudrate or IP port
uint32_t m_baudrate;
/// LiDAR intensity
bool m_intensities = false;
/// LiDAR intensity bit
int m_intensityBit = 0;
/// LiDAR Point pointer
node_info *scan_node_buf = nullptr;
/// LiDAR scan count
size_t scan_node_count = 0; //LiDAR Scan Count
/// package sample index
uint16_t nodeIndex = 0;
///
int retryCount = 0;
/// auto reconnect
bool isAutoReconnect = true;
/// auto connecting state
bool isAutoconnting = false;
lidarConfig m_config;
/// number of last error
DriverError m_driverErrno;
/// invalid node count
int m_InvalidNodeCount = 0;
size_t m_BufferSize = 0;
};
} // common
} // core
} // ydlidar

28
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_config.h Normal file
View File

@@ -0,0 +1,28 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2020 EAIBOT. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#pragma once
#define YDLIDAR_SDK_VERSION 1.2.9
#define YDLIDAR_SDK_VERSION_STR "1.2.9"

121
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_datatype.h Normal file
View File

@@ -0,0 +1,121 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, EAIBOT, Inc.
* 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (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 <core/base/datatype.h>
#include <vector>
#include "ydlidar_def.h"
#define MAX_DEBUG_INDEX 14
/**
* @brief The Laser Debug struct
*/
typedef struct {
uint8_t cVer;
uint8_t debug2;
uint8_t hfVer;
uint8_t fVer;
uint8_t month;
uint8_t day;
uint8_t year;
uint8_t numH;
uint8_t numL;
uint8_t health;
uint8_t model; //雷达型号
uint8_t maxIndex;
} LaserDebug;
/**
* @brief The Laser Scan Data struct
* @par usage
* @code
* LaserScan data;
* for(int i = 0; i < data.points.size(); i++) {
* //current LiDAR angle
* float angle = data.points[i].angle;
* //current LiDAR range
* float range = data.points[i].range;
* //current LiDAR intensity
* float intensity = data.points[i].intensity;
* //current LiDAR point stamp
* uint64_t timestamp = data.stamp + i * data.config.time_increment * 1e9;
* }
* LaserScanDestroy(&data);
* @endcode
* @par convert to ROS sensor_msgs::LaserScan
* @code
* LaserScan scan;
* sensor_msgs::LaserScan scan_msg;
* std::string frame_id = "laser_frame";
* ros::Time start_scan_time;
* start_scan_time.sec = scan.stamp/1000000000ul;
* start_scan_time.nsec = scan.stamp%1000000000ul;
* scan_msg.header.stamp = start_scan_time;
* scan_msg.header.frame_id = frame_id;
* scan_msg.angle_min =(scan.config.min_angle);
* scan_msg.angle_max = (scan.config.max_angle);
* scan_msg.angle_increment = (scan.config.angle_increment);
* scan_msg.scan_time = scan.config.scan_time;
* scan_msg.time_increment = scan.config.time_increment;
* scan_msg.range_min = (scan.config.min_range);
* scan_msg.range_max = (scan.config.max_range);
* int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1;
* scan_msg.ranges.resize(size);
* scan_msg.intensities.resize(size);
* for(int i=0; i < scan.points.size(); i++) {
* int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment);
* if(index >=0 && index < size) {
* scan_msg.ranges[index] = scan.points[i].range;
* scan_msg.intensities[index] = scan.points[i].intensity;
* }
* }
* @endcode
*/
typedef struct {
// System time when first range was measured in nanoseconds
uint64_t stamp = 0;
//转速
float scanFreq = .0;
//采样率
float sampleRate = .0;
// Array of lidar points
std::vector<LaserPoint> points;
int size = 0; //实际点数（固定分辨率时点数与实际点数不符）
// Configuration of scan
LaserConfig config;
int moduleNum = 0;
uint16_t envFlag = 0; //环境标记（目前只针对GS2）
} LaserScan;

39
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.cpp Normal file
View File

@@ -0,0 +1,39 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2020 EAIBOT. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <core/common/ydlidar_def.h>
#include <string.h>
void LaserFanInit(LaserFan *to_init) {
if (to_init) {
memset(to_init, 0, sizeof(LaserFan));
}
}
void LaserFanDestroy(LaserFan *to_destroy) {
if (to_destroy && to_destroy->points) {
free(to_destroy->points);
to_destroy->points = NULL;
}
}

246
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.h Normal file
View File

@@ -0,0 +1,246 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2020 EAIBOT. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef YDLIDAR_DEF_H_
#define YDLIDAR_DEF_H_
#include <stdint.h>
#include <stdlib.h>
#include <core/base/typedef.h>
#include <core/base/utils.h>
#ifdef __cplusplus
extern "C" {
#endif
/** Device Type ID */
typedef enum {
YDLIDAR_TYPE_SERIAL = 0x0,/**< serial type.*/
YDLIDAR_TYPE_TCP = 0x1,/**< socket tcp type.*/
YDLIDAR_TYPC_UDP = 0x2,/**< socket udp type.*/
} DeviceTypeID;
/** Lidar Type ID */
typedef enum {
TYPE_TOF = 0, //TG系列雷达
TYPE_TRIANGLE = 1, //S2、S2 Pro、S4、G4、G6、G2、Tmini等三角协议雷达
TYPE_TOF_NET = 2, //T系列雷达
TYPE_GS = 3, //GS系列雷达（目前只有GS2）
TYPE_SCL = 4, //SCL雷达
TYPE_SDM = 5, //SDM15单点雷达
TYPE_SDM18 = 6, //SDM18单点雷达
TYPE_TIA = 7, //TIA系列雷达
TYPE_Tail,
} LidarTypeID;
/** Lidar Properties,Lidar Can set and get parameter property index.\n
* float properties must be float type, not double type.
*/
typedef enum {
/* char* properties */
LidarPropSerialPort = 0,/**< Lidar serial port or network ipaddress */
LidarPropIgnoreArray,/**< Lidar ignore angle array */
/* int properties */
LidarPropSerialBaudrate = 10,/**< lidar serial baudrate or network port */
LidarPropLidarType,/**< lidar type code */
LidarPropDeviceType,/**< lidar connection type code */
LidarPropSampleRate,/**< lidar sample rate */
LidarPropAbnormalCheckCount,/**< abnormal maximum check times */
LidarPropIntenstiyBit,/**< lidar intensity bit count */
/* float properties */
LidarPropMaxRange = 20,/**< lidar maximum range */
LidarPropMinRange,/**< lidar minimum range */
LidarPropMaxAngle,/**< lidar maximum angle */
LidarPropMinAngle,/**< lidar minimum angle */
LidarPropScanFrequency,/**< lidar scanning frequency */
/* bool properties */
LidarPropFixedResolution = 30,/**< fixed angle resolution flag */
LidarPropReversion,/**< lidar reversion flag */
LidarPropInverted,/**< lidar inverted flag */
LidarPropAutoReconnect,/**< lidar hot plug flag */
LidarPropSingleChannel,/**< lidar single-channel flag */
LidarPropIntenstiy,/**< lidar intensity flag */
LidarPropSupportMotorDtrCtrl,/**< lidar support motor Dtr ctrl flag */
LidarPropSupportHeartBeat,/**< lidar support heartbeat flag */
} LidarProperty;
/// lidar instance
typedef struct {
void *lidar;///< CYdLidar instance
} YDLidar;
typedef enum {
NoError = 0,
DeviceNotFoundError,
PermissionError,
UnsupportedOperationError,
UnknownError,
TimeoutError,
NotOpenError,
BlockError,
NotBufferError,
TrembleError,
LaserFailureError,
} DriverError;
#pragma pack(1)
/**
* @brief The Laser Point struct
* @note angle unit: rad.\n
* range unit: meter.\n
*/
typedef struct {
/// lidar angle. unit(rad)
float angle;
/// lidar range. unit(m)
float range;
/// lidar intensity
float intensity;
} LaserPoint;
/**
* @brief A struct for returning configuration from the YDLIDAR
* @note angle unit: rad.\n
* time unit: second.\n
* range unit: meter.\n
*/
typedef struct {
/// Start angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing YDLIDAR from the top.
float min_angle;
/// Stop angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing YDLIDAR from the top.
float max_angle;
/// angle resoltuion [rad]
float angle_increment;
/// Scan resoltuion [s]
float time_increment;
/// Time between scans 扫描时长，单位秒
float scan_time;
/// Minimum range [m]
float min_range;
/// Maximum range [m]
float max_range;
} LaserConfig;
/**
* @brief The Laser Scan Data struct
* @par usage
* @code
* LaserScan data;
* for(int i = 0; i < data.npoints; i++) {
* //current LiDAR angle
* float angle = data.points[i].angle;
* //current LiDAR range
* float range = data.points[i].range;
* //current LiDAR intensity
* float intensity = data.points[i].intensity;
* //current LiDAR point stamp
* uint64_t timestamp = data.stamp + i * data.config.time_increment * 1e9;
* }
* LaserScanDestroy(&data);
* @endcode
* @par convert to ROS sensor_msgs::LaserScan
* @code
* LaserScan scan;
* sensor_msgs::LaserScan scan_msg;
* std::string frame_id = "laser_frame";
* ros::Time start_scan_time;
* start_scan_time.sec = scan.stamp/1000000000ul;
* start_scan_time.nsec = scan.stamp%1000000000ul;
* scan_msg.header.stamp = start_scan_time;
* scan_msg.header.frame_id = frame_id;
* scan_msg.angle_min =(scan.config.min_angle);
* scan_msg.angle_max = (scan.config.max_angle);
* scan_msg.angle_increment = (scan.config.angle_increment);
* scan_msg.scan_time = scan.config.scan_time;
* scan_msg.time_increment = scan.config.time_increment;
* scan_msg.range_min = (scan.config.min_range);
* scan_msg.range_max = (scan.config.max_range);
* int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1;
* scan_msg.ranges.resize(size);
* scan_msg.intensities.resize(size);
* for(int i=0; i < scan.npoints; i++) {
* int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment);
* if(index >=0 && index < size) {
* scan_msg.ranges[index] = scan.points[i].range;
* scan_msg.intensities[index] = scan.points[i].intensity;
* }
* }
* LaserScanDestroy(&scan);
* @endcode
*/
typedef struct {
/// System time when first range was measured in nanoseconds
uint64_t stamp;///< ns
/// Array of lidar points
uint32_t npoints;
LaserPoint *points;
/// Configuration of scan
LaserConfig config;
} LaserFan;
/**
* @brief c string
*/
typedef struct {
/// data
char data[50];
} string_t;
/**
* @brief lidar ports
*/
typedef struct {
string_t port[8];
} LidarPort;
/** The numeric version information struct. */
typedef struct {
uint8_t hardware; /**< Hardware version*/
uint8_t soft_major; /**< major number */
uint8_t soft_minor; /**< minor number */
uint8_t soft_patch; /**< patch number */
uint8_t sn[16]; /**< serial number*/
} LidarVersion;
#pragma pack()
/**
* @brief initialize LaserFan
* @param to_init
*/
YDLIDAR_API void LaserFanInit(LaserFan *to_init);
/**
* Destroy an instance of LaserFan points
*/
YDLIDAR_API void LaserFanDestroy(LaserFan *to_destroy);
#ifdef __cplusplus
}
#endif
#endif // YDLIDAR_DEF_H_

1181
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_help.h Normal file
View File

File diff suppressed because it is too large Load Diff

531
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_protocol.h Normal file
View File

@@ -0,0 +1,531 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, EAIBOT, Inc.
* 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (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 <core/base/v8stdint.h>
#include <vector>
#include <functional>
/// Count the number of elements in a statically allocated array.
#if !defined(_countof)
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif
#ifndef M_PI
#define M_PI 3.1415926
#endif
//浮点型判断是否为0
#define ISZERO(v) (abs(v) < 1e-6)
#define SUNNOISEINTENSITY 0x03 //sun noise flag constant
#define GLASSNOISEINTENSITY 0x02 //glass noise flag constant
//LIDAR CMD Protocol
#define LIDAR_CMD_STOP 0x65
#define LIDAR_CMD_SCAN 0x60
#define LIDAR_CMD_FORCE_SCAN 0x61
#define LIDAR_CMD_RESET 0x80
#define LIDAR_CMD_FORCE_STOP 0x00
#define LIDAR_CMD_GET_EAI 0x55
#define LIDAR_CMD_GET_DEVICE_INFO 0x90
#define LIDAR_CMD_GET_DEVICE_HEALTH 0x92
#define LIDAR_CMD_SYNC_BYTE 0xA5
#define LIDAR_CMDFLAG_HAS_PAYLOAD 0x80
#define LIDAR_RESP_SYNCBIT (0x1<<0)
#define LIDAR_RESP_QUALITY_SHIFT 2
#define LIDAR_RESP_CHECKBIT (0x1<<0)
#define LIDAR_RESP_ANGLE_SHIFT 1
#define LIDAR_RESP_DIST_SHIFT 2
#define LIDAR_RESP_ANGLE_SAMPLE_SHIFT 8
#define LIDAR_ANS_SYNC_BYTE1 0xA5
#define LIDAR_ANS_SYNC_BYTE2 0x5A
#define LIDAR_ANS_TYPE_DEVINFO 0x04
#define LIDAR_ANS_TYPE_DEVHEALTH 0x06
#define LIDAR_ANS_TYPE_PITCH 0x10
#define LIDAR_ANS_TYPE_MEASUREMENT 0x81
#define LIDAR_CMD_RUN_POSITIVE 0x06
#define LIDAR_CMD_RUN_INVERSION 0x07
#define LIDAR_CMD_SET_AIMSPEED_ADDMIC 0x09
#define LIDAR_CMD_SET_AIMSPEED_DISMIC 0x0A
#define LIDAR_CMD_SET_AIMSPEED_ADD 0x0B
#define LIDAR_CMD_SET_AIMSPEED_DIS 0x0C
#define LIDAR_CMD_GET_AIMSPEED 0x0D
#define LIDAR_CMD_SET_SAMPLING_RATE 0xD0
#define LIDAR_CMD_GET_SAMPLING_RATE 0xD1
#define LIDAR_STATUS_OK 0x0
#define LIDAR_STATUS_WARNING 0x1
#define LIDAR_STATUS_ERROR 0x2
#define LIDAR_CMD_ENABLE_LOW_POWER 0x01
#define LIDAR_CMD_DISABLE_LOW_POWER 0x02
#define LIDAR_CMD_STATE_MODEL_MOTOR 0x05
#define LIDAR_CMD_ENABLE_CONST_FREQ 0x0E
#define LIDAR_CMD_DISABLE_CONST_FREQ 0x0F
#define LIDAR_CMD_GET_OFFSET_ANGLE 0x93
#define LIDAR_CMD_SAVE_SET_EXPOSURE 0x94
#define LIDAR_CMD_SET_LOW_EXPOSURE 0x95
#define LIDAR_CMD_ADD_EXPOSURE 0x96
#define LIDAR_CMD_DIS_EXPOSURE 0x97
#define LIDAR_CMD_SET_HEART_BEAT 0xD9
#define LIDAR_CMD_GETPITCH 0x98 //获取俯仰角
//GS命令
#define GS_LIDAR_CMD_GET_ADDRESS 0x60
#define GS_LIDAR_CMD_GET_PARAMETER 0x61
#define GS_LIDAR_CMD_GET_VERSION 0x62
#define GS_LIDAR_CMD_GET_VERSION3 0x6B
#define GS_LIDAR_CMD_SCAN 0x63
#define GS_LIDAR_ANS_SCAN 0x63
#define GS_LIDAR_CMD_STOP 0x64
#define GS_LIDAR_CMD_RESET 0x67
#define GS_LIDAR_CMD_SET_MODE 0x69
#define GS_LIDAR_CMD_SET_BIAS 0xD9
#define GS_LIDAR_CMD_SET_DEBUG_MODE 0xF0
//GS
#define Angle_Px 1.22
#define Angle_Py 5.315
#define Angle_PAngle 22.5 //GS2
#define Angle_PAngle2 19.0 //GS5
#define GS_PACKHEADSIZE 8
#define GS_PACKMAXNODES 160 //GS数据包中最大点云数
#define SDK_SNLEN 16 //序列号长度
/// Default Node Quality
#define Node_Default_Quality (10)
/// Starting Node
#define NODE_SYNC 1
/// Normal Node
#define NODE_UNSYNC 2
/// Package Header
#define PH 0x55AA
#define PH1 0xAA
#define PH2 0x55 //AA55是点云数据
#define PH3 0x66 //AA66是时间戳数据
//Package
#define TRI_PACKHEADSIZE 10
#define TRI_PACKMAXNODES 40 //单包最大点数
#define TIA_PACKWIDTH 12 //TIA单包行数
#define TIA_PACKHEIGHT 16 //TIA单包行数
#define TIA_PACKMAXBUFFS (TIA_PACKWIDTH * (4 + TIA_PACKHEIGHT * 4) + 4 + 4) //TIA-H单包最大字节数
#define TIA_PACKMAXBUFFS2 (TIA_PACKMAXBUFFS + 4) //TIA单包最大字节数
#define TIA_PACKMAXNODES (TIA_PACKWIDTH * TIA_PACKHEIGHT) //TIA单包最大点数
//模组地址
#define LIDAR_MODULE_1 0x01
#define LIDAR_MODULE_2 0x02
#define LIDAR_MODULE_3 0x04
#define LIDAR_MODULE_ALL 0x00
#define LIDAR_MAXCOUNT 3 //最大模组数
#define LIDAR_PACKMAXNODES TRI_PACKMAXNODES //单包最大点数
#define LIDAR_MAXNODES 5000 //最大点数
#define FREINDEX 0
#define USERVERSIONNDEX 1
#define HEALTHINDEX 3
//超时定义
#define TIMEOUT_100 100
#define TIMEOUT_300 300
#define TIMEOUT_500 500 //500ms
#define TIMEOUT_1S 1000
#define TIMEOUT_2S 2000
#define SDK_TIMEOUT TIMEOUT_1S //默认超时时间
//角度定义
#define SDK_ANGLE360 360.0f
#define SDK_ANGLE180 180.0f
#define SDK_ANGLE90 90.0f
#define SDK_ANGLE0 0.0f
/// CT Package Type
typedef enum {
CT_Normal = 0,///< Normal package
CT_RingStart = 1,///< Starting package
CT_Tail,
} CT;
//雷达协议类型
typedef enum {
Protocol_V1 = 0, //V1 version
Protocol_V2 = 1, //V2 version
} ProtocolVer;
//设备所属平台类型
enum EaiPlatformType
{
EPT_None = 0x00, //无
EPT_Module = 0x01, //模组
EPT_Base = 0x02, //底板
EPT_All = (EPT_Module | EPT_Base), //所有
};
#if defined(_WIN32)
#pragma pack(1)
#endif
//雷达节点信息
struct node_info {
uint8_t sync; //首包标记
uint8_t is; //抗干扰标志
uint16_t qual; //信号强度
uint16_t angle; //角度值（°）
uint16_t dist; //距离值
uint64_t stamp; //时间戳
uint32_t delayTime; //delay time
uint8_t scanFreq; //扫描频率
uint8_t debugInfo; //debug information
uint8_t index; //包序号
uint8_t error; //error package state
} __attribute__((packed));
#define SDKNODESIZE sizeof(node_info)
//package node info
struct tri_node {
uint16_t dist; //range
} __attribute__((packed));
//package node info
struct tri_node2 {
uint8_t qual;///< intensity
uint16_t dist;///< range
} __attribute__((packed));
// TOF Intensity package node info
struct tof_node {
uint16_t qual;
uint16_t dist;
} __attribute__((packed));
//LiDAR Normal Nodes package
struct tri_node_package {
uint16_t head;///< package header
uint8_t ct;///< package ct
uint8_t count; ///< package number
uint16_t firstAngle;///< first sample angle
uint16_t lastAngle;///< last sample angle
uint16_t cs;///< checksum
uint16_t nodes[TRI_PACKMAXNODES];
} __attribute__((packed));
//LiDAR Intensity Nodes Package
struct tri_node_package2 {
uint16_t head;///< package header
uint8_t ct;///< package ct
uint8_t count;///< package number
uint16_t firstAngle;///< first sample angle
uint16_t lastAngle;///< last sample angle
uint16_t cs;///< checksum
tri_node2 nodes[TRI_PACKMAXNODES];
} __attribute__((packed));
// TOF LiDAR Intensity Nodes Package
struct tof_node_package {
uint16_t head;
uint8_t ct;
uint8_t count;
uint16_t firstAngle;
uint16_t lastAngle;
uint16_t cs;
tof_node nodes[TRI_PACKMAXNODES];
} __attribute__((packed));
//时间戳结构体
struct stamp_package {
uint8_t flag1; //包头标记1
uint8_t flag2; //包头标记2
uint8_t cs; //校验和
uint32_t stamp; //时间戳
uint8_t reserved; //保留字段
} __attribute__((packed));
#define SIZE_STAMPPACKAGE sizeof(stamp_package)
//设备信息结构体
struct device_info {
uint8_t model; //雷达型号码
uint16_t firmware_version; //固件版本
uint8_t hardware_version; //硬件版本
uint8_t serialnum[SDK_SNLEN]; //序列号
} __attribute__((packed));
#define DEVICEINFOSIZE sizeof(device_info)
//设备信息结构体（带模组序号）
struct device_info_ex {
uint8_t id = 0;
device_info di = {0};
};
/// LiDAR Health Information
struct device_health {
uint8_t status; ///< health state
uint16_t error_code; ///< error code
} __attribute__((packed)) ;
/// LiDAR sampling Rate struct
struct sampling_rate {
uint8_t rate; ///< sample rate
} __attribute__((packed)) ;
/// LiDAR scan frequency struct
struct scan_frequency {
uint32_t frequency; ///< scan frequency
} __attribute__((packed)) ;
struct scan_rotation {
uint8_t rotation;
} __attribute__((packed)) ;
/// LiDAR Exposure struct
struct scan_exposure {
uint8_t exposure; ///< low exposure
} __attribute__((packed)) ;
/// LiDAR Heart beat struct
struct scan_heart_beat {
uint8_t enable; ///< heart beat
} __attribute__((packed));
struct scan_points {
uint8_t flag;
} __attribute__((packed)) ;
struct function_state {
uint8_t state;
} __attribute__((packed)) ;
/// LiDAR Zero Offset Angle
struct offset_angle {
int32_t angle;
} __attribute__((packed)) ;
/// LiDAR request command packet
struct cmd_packet {
uint8_t syncByte;
uint8_t cmd_flag;
uint8_t size;
uint8_t data;
} __attribute__((packed)) ;
/// LiDAR response Header
struct lidar_ans_header {
uint8_t syncByte1;
uint8_t syncByte2;
uint32_t size: 30;
uint32_t subType: 2;
uint8_t type;
} __attribute__((packed));
#define TRIRESPHEADSIZE sizeof(lidar_ans_header) //定义通用响应头大小
//GS单帧数据
struct gs_packages {
int frameNum;
int moduleNum;
bool left = false;
bool right = false;
node_info points[GS_PACKMAXNODES];
} __attribute__((packed));
struct gs_module_nodes {
int moduleNum = 0;
int pointCount = 0;
node_info points[GS_PACKMAXNODES];
} __attribute__((packed));
//GS点数据结构
struct gs_node {
// uint16_t dist : 9;
// uint16_t qual : 7;
uint16_t node;
} __attribute__((packed));
#define GSNODESIZE sizeof(gs_node) //定义GS点大小
//GS单包数据结构
struct gs_node_package {
uint32_t head;
uint8_t address;
uint8_t ct;
uint16_t size;
uint16_t env;
gs_node nodes[GS_PACKMAXNODES];
uint8_t cs;
} __attribute__((packed));
#define GSPACKSIZE sizeof(gs_node_package) //定义GS点大小
//GS设备参数
struct gs_device_para {
uint16_t k0;
uint16_t b0;
uint16_t k1;
uint16_t b1;
int8_t bias;
uint8_t crc;
} __attribute__((packed));
//GS包头
struct gs_package_head {
uint8_t syncByte0;
uint8_t syncByte1;
uint8_t syncByte2;
uint8_t syncByte3;
uint8_t address;
uint8_t type;
uint16_t size;
} __attribute__((packed));
#define GSPACKEGEHEADSIZE sizeof(gs_package_head)
//GS系列设备信息
struct gs_device_info {
uint8_t hwVersion; //硬件版本号
uint16_t fwVersion; //固件版本号
uint8_t sn[16]; //序列号
} __attribute__((packed));
#define GSDEVINFOSIZE sizeof(gs_device_info)
//GS系列设备信息（带雷达型号）
struct gs_device_info2 {
uint8_t hwVersion; //硬件版本号
uint16_t fwVersion; //固件版本号
uint8_t model; //型号
uint8_t sn[16]; //序列号
} __attribute__((packed));
#define GSDEVINFO2SIZE sizeof(gs_device_info2)
#if defined(_WIN32)
#pragma pack()
#endif
/**
* @brief UDP Data format
*/
typedef struct _dataFrame {
uint16_t frameHead;
uint8_t deviceType;
uint8_t frameType;
uint8_t dataIndex;
uint8_t frameIndex;
uint32_t timestamp;
uint8_t headFrameFlag;
uint8_t dataFormat;
uint8_t disScale;
uint32_t startAngle;
uint32_t dataNum;
uint32_t frameCrc;
uint8_t frameBuf[2048];
} dataFrame;
/**
* @class lidarConfig
* @brief Structure containing scan configuration.
*
* @author jzhang
*/
typedef struct _lidarConfig {
/**
* @brief Scanning enable.
*/
int laser_en;
/**
* @brief rotate enable.
*/
int motor_en;
/**
* @brief motor RPM.
*/
int motor_rpm;
/**
* @brief start FOV angle.
*/
int fov_start;
/**
* @brief end FOV angle.
*/
int fov_end;
/**
* @brief data receive interface, USB or Ethernet.
*/
int trans_sel;
/**
* @brief data receive IP.
*/
char dataRecvIp[16];
/**
* @brief data receive PORT.
*/
int dataRecvPort;
/**
* @brief device network config, HDCP or Manual.
*/
int dhcp_en;
/**
* @brief device IP.
*/
char deviceIp[16];
/**
* @brief device netmask.
*/
char deviceNetmask[16];
/**
* @brief device gateway ip.
*/
char deviceGatewayIp[16];
int laserScanFrequency;
/**
* @brief correction_angle
*/
int correction_angle;
/**
* @brief correction_distance
*/
int correction_distance;
} lidarConfig;
#include "ydlidar_datatype.h"

5
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/CMakeLists.txt Normal file
View File

@@ -0,0 +1,5 @@
aux_include_directory(. HDRS)
aux_source_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})

3119
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.c Normal file
View File

File diff suppressed because it is too large Load Diff

300
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.h Normal file
View File

@@ -0,0 +1,300 @@
/*
Copyright (c) 2009-2017 Dave Gamble and cJSON contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef cJSON__h
#define cJSON__h
#ifdef __cplusplus
extern "C"
{
#endif
#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32))
#define __WINDOWS__
#endif
#ifdef __WINDOWS__
/* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options:
CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols
CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default)
CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol
For *nix builds that support visibility attribute, you can define similar behavior by
setting default visibility to hidden by adding
-fvisibility=hidden (for gcc)
or
-xldscope=hidden (for sun cc)
to CFLAGS
then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does
*/
#define CJSON_CDECL __cdecl
#define CJSON_STDCALL __stdcall
/* export symbols by default, this is necessary for copy pasting the C and header file */
#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS)
#define CJSON_EXPORT_SYMBOLS
#endif
#if defined(CJSON_HIDE_SYMBOLS)
#define CJSON_PUBLIC(type) type CJSON_STDCALL
#elif defined(CJSON_EXPORT_SYMBOLS)
#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL
#elif defined(CJSON_IMPORT_SYMBOLS)
#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL
#endif
#else /* !__WINDOWS__ */
#define CJSON_CDECL
#define CJSON_STDCALL
#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined (__SUNPRO_C)) && defined(CJSON_API_VISIBILITY)
#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type
#else
#define CJSON_PUBLIC(type) type
#endif
#endif
/* project version */
#define CJSON_VERSION_MAJOR 1
#define CJSON_VERSION_MINOR 7
#define CJSON_VERSION_PATCH 15
#include <stddef.h>
/* cJSON Types: */
#define cJSON_Invalid (0)
#define cJSON_False (1 << 0)
#define cJSON_True (1 << 1)
#define cJSON_NULL (1 << 2)
#define cJSON_Number (1 << 3)
#define cJSON_String (1 << 4)
#define cJSON_Array (1 << 5)
#define cJSON_Object (1 << 6)
#define cJSON_Raw (1 << 7) /* raw json */
#define cJSON_IsReference 256
#define cJSON_StringIsConst 512
/* The cJSON structure: */
typedef struct cJSON
{
/* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */
struct cJSON *next;
struct cJSON *prev;
/* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */
struct cJSON *child;
/* The type of the item, as above. */
int type;
/* The item's string, if type==cJSON_String and type == cJSON_Raw */
char *valuestring;
/* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */
int valueint;
/* The item's number, if type==cJSON_Number */
double valuedouble;
/* The item's name string, if this item is the child of, or is in the list of subitems of an object. */
char *string;
} cJSON;
typedef struct cJSON_Hooks
{
/* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */
void *(CJSON_CDECL *malloc_fn)(size_t sz);
void (CJSON_CDECL *free_fn)(void *ptr);
} cJSON_Hooks;
typedef int cJSON_bool;
/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them.
* This is to prevent stack overflows. */
#ifndef CJSON_NESTING_LIMIT
#define CJSON_NESTING_LIMIT 1000
#endif
/* returns the version of cJSON as a string */
CJSON_PUBLIC(const char*) cJSON_Version(void);
/* Supply malloc, realloc and free functions to cJSON */
CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks* hooks);
/* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */
/* Supply a block of JSON, and this returns a cJSON object you can interrogate. */
CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value);
CJSON_PUBLIC(cJSON *) cJSON_ParseWithLength(const char *value, size_t buffer_length);
/* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */
/* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */
CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated);
CJSON_PUBLIC(cJSON *) cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated);
/* Render a cJSON entity to text for transfer/storage. */
CJSON_PUBLIC(char *) cJSON_Print(const cJSON *item);
/* Render a cJSON entity to text for transfer/storage without any formatting. */
CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON *item);
/* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */
CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt);
/* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */
/* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */
CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format);
/* Delete a cJSON entity and all subentities. */
CJSON_PUBLIC(void) cJSON_Delete(cJSON *item);
/* Returns the number of items in an array (or object). */
CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array);
/* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */
CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON *array, int index);
/* Get item "string" from object. Case insensitive. */
CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, const char * const string);
CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive(const cJSON * const object, const char * const string);
CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string);
/* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */
CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void);
/* Check item type and return its value */
CJSON_PUBLIC(char *) cJSON_GetStringValue(const cJSON * const item);
CJSON_PUBLIC(double) cJSON_GetNumberValue(const cJSON * const item);
/* These functions check the type of an item */
CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item);
/* These calls create a cJSON item of the appropriate type. */
CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean);
CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num);
CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string);
/* raw json */
CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw);
CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void);
/* Create a string where valuestring references a string so
* it will not be freed by cJSON_Delete */
CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string);
/* Create an object/array that only references it's elements so
* they will not be freed by cJSON_Delete */
CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON *child);
CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON *child);
/* These utilities create an Array of count items.
* The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/
CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count);
CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count);
CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count);
CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char *const *strings, int count);
/* Append item to the specified array/object. */
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToArray(cJSON *array, cJSON *item);
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item);
/* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object.
* WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before
* writing to `item->string` */
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item);
/* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item);
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item);
/* Remove/Detach items from Arrays/Objects. */
CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON *parent, cJSON * const item);
CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON *array, int which);
CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which);
CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON *object, const char *string);
CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string);
CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string);
CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string);
/* Update array items. */
CJSON_PUBLIC(cJSON_bool) cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, cJSON * const item, cJSON * replacement);
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem);
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObject(cJSON *object,const char *string,cJSON *newitem);
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object,const char *string,cJSON *newitem);
/* Duplicate a cJSON item */
CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON *item, cJSON_bool recurse);
/* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will
* need to be released. With recurse!=0, it will duplicate any children connected to the item.
* The item->next and ->prev pointers are always zero on return from Duplicate. */
/* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal.
* case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */
CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, const cJSON * const b, const cJSON_bool case_sensitive);
/* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings.
* The input pointer json cannot point to a read-only address area, such as a string constant,
* but should point to a readable and writable address area. */
CJSON_PUBLIC(void) cJSON_Minify(char *json);
/* Helper functions for creating and adding items to an object at the same time.
* They return the added item or NULL on failure. */
CJSON_PUBLIC(cJSON*) cJSON_AddNullToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddTrueToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddFalseToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddBoolToObject(cJSON * const object, const char * const name, const cJSON_bool boolean);
CJSON_PUBLIC(cJSON*) cJSON_AddNumberToObject(cJSON * const object, const char * const name, const double number);
CJSON_PUBLIC(cJSON*) cJSON_AddStringToObject(cJSON * const object, const char * const name, const char * const string);
CJSON_PUBLIC(cJSON*) cJSON_AddRawToObject(cJSON * const object, const char * const name, const char * const raw);
CJSON_PUBLIC(cJSON*) cJSON_AddObjectToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddArrayToObject(cJSON * const object, const char * const name);
/* When assigning an integer value, it needs to be propagated to valuedouble too. */
#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number))
/* helper for the cJSON_SetNumberValue macro */
CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number);
#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number))
/* Change the valuestring of a cJSON_String object, only takes effect when type of object is cJSON_String */
CJSON_PUBLIC(char*) cJSON_SetValuestring(cJSON *object, const char *valuestring);
/* If the object is not a boolean type this does nothing and returns cJSON_Invalid else it returns the new type*/
#define cJSON_SetBoolValue(object, boolValue) ( \
(object != NULL && ((object)->type & (cJSON_False|cJSON_True))) ? \
(object)->type=((object)->type &(~(cJSON_False|cJSON_True)))|((boolValue)?cJSON_True:cJSON_False) : \
cJSON_Invalid\
)
/* Macro for iterating over an array or object */
#define cJSON_ArrayForEach(element, array) for(element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next)
/* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */
CJSON_PUBLIC(void *) cJSON_malloc(size_t size);
CJSON_PUBLIC(void) cJSON_free(void *object);
#ifdef __cplusplus
}
#endif
#endif

5
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/CMakeLists.txt Normal file
View File

@@ -0,0 +1,5 @@
aux_include_directory(. HDRS)
aux_source_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})

297
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/angles.h Normal file
View File

@@ -0,0 +1,297 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (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 <algorithm>
#include <cmath>
#ifndef M_PI
#define M_PI 3.1415926
#endif
namespace ydlidar {
namespace core {
namespace math {
/*!
* \brief Convert degrees to radians
*/
static inline double from_degrees(double degrees) {
return degrees * M_PI / 180.0;
}
/*!
* \brief Convert radians to degrees
*/
static inline double to_degrees(double radians) {
return radians * 180.0 / M_PI;
}
/*!
* \brief normalize_angle_positive
*
* Normalizes the angle to be 0 to 2*M_PI
* It takes and returns radians.
*/
static inline double normalize_angle_positive(double angle) {
return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI);
}
/*!
* \brief normalize_angle_positive_from_degree
*
* Normalizes the angle to be 0 to 360
* It takes and returns degree.
*/
static inline double normalize_angle_positive_from_degree(double angle) {
double degree = angle;
while (degree >= 360.0) {
degree -= 360.0;
}
while (degree < 0) {
degree += 360.0;
}
return degree;
}
/*!
* \brief normalize
*
* Normalizes the angle to be -M_PI circle to +M_PI circle
* It takes and returns radians.
*
*/
static inline double normalize_angle(double angle) {
double a = normalize_angle_positive(angle);
if (a > M_PI) {
a -= 2.0 * M_PI;
}
return a;
}
/*!
* \function
* \brief shortest_angular_distance
*
* Given 2 angles, this returns the shortest angular
* difference. The inputs and ouputs are of course radians.
*
* The result
* would always be -pi <= result <= pi. Adding the result
* to "from" will always get you an equivelent angle to "to".
*/
static inline double shortest_angular_distance(double from, double to) {
return normalize_angle(to - from);
}
/*!
* \function
*
* \brief returns the angle in [-2*M_PI, 2*M_PI] going the other way along the unit circle.
* \param angle The angle to which you want to turn in the range [-2*M_PI, 2*M_PI]
* E.g. two_pi_complement(-M_PI/4) returns 7_M_PI/4
* two_pi_complement(M_PI/4) returns -7*M_PI/4
*
*/
static inline double two_pi_complement(double angle) {
//check input conditions
if (angle > 2 * M_PI || angle < -2.0 * M_PI) {
angle = fmod(angle, 2.0 * M_PI);
}
if (angle < 0) {
return (2 * M_PI + angle);
} else if (angle > 0) {
return (-2 * M_PI + angle);
}
return (2 * M_PI);
}
/*!
* \function
*
* \brief This function is only intended for internal use and not intended for external use. If you do use it, read the documentation very carefully. Returns the min and max amount (in radians) that can be moved from "from" angle to "left_limit" and "right_limit".
* \return returns false if "from" angle does not lie in the interval [left_limit,right_limit]
* \param from - "from" angle - must lie in [-M_PI, M_PI)
* \param left_limit - left limit of valid interval for angular position - must lie in [-M_PI, M_PI], left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
* \param right_limit - right limit of valid interval for angular position - must lie in [-M_PI, M_PI], left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
* \param result_min_delta - minimum (delta) angle (in radians) that can be moved from "from" position before hitting the joint stop
* \param result_max_delta - maximum (delta) angle (in radians) that can be movedd from "from" position before hitting the joint stop
*/
static bool find_min_max_delta(double from, double left_limit,
double right_limit, double &result_min_delta, double &result_max_delta) {
double delta[4];
delta[0] = shortest_angular_distance(from, left_limit);
delta[1] = shortest_angular_distance(from, right_limit);
delta[2] = two_pi_complement(delta[0]);
delta[3] = two_pi_complement(delta[1]);
if (fabs(delta[0]) < 1e-6) {
result_min_delta = delta[0];
result_max_delta = std::max<double>(delta[1], delta[3]);
return true;
}
if (fabs(delta[1]) < 1e-6) {
result_max_delta = delta[1];
result_min_delta = std::min<double>(delta[0], delta[2]);
return true;
}
double delta_min = delta[0];
double delta_min_2pi = delta[2];
if (delta[2] < delta_min) {
delta_min = delta[2];
delta_min_2pi = delta[0];
}
double delta_max = delta[1];
double delta_max_2pi = delta[3];
if (delta[3] > delta_max) {
delta_max = delta[3];
delta_max_2pi = delta[1];
}
// printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
if ((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi)) {
result_min_delta = delta_max_2pi;
result_max_delta = delta_min_2pi;
if (left_limit == -M_PI && right_limit == M_PI) {
return true;
} else {
return false;
}
}
result_min_delta = delta_min;
result_max_delta = delta_max;
return true;
}
/*!
* \function
*
* \brief Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by left_limit and right_limit.
* The valid interval of angular positions is [left_limit,right_limit]. E.g., [-0.25,0.25] is a 0.5 radians wide interval that contains 0.
* But [0.25,-0.25] is a 2*M_PI-0.5 wide interval that contains M_PI (but not 0).
* The value of shortest_angle is the angular difference between "from" and "to" that lies within the defined valid interval.
* E.g. shortest_angular_distance_with_limits(-0.5,0.5,0.25,-0.25,ss) evaluates ss to 2*M_PI-1.0 and returns true while
* shortest_angular_distance_with_limits(-0.5,0.5,-0.25,0.25,ss) returns false since -0.5 and 0.5 do not lie in the interval [-0.25,0.25]
*
* \return true if "from" and "to" positions are within the limit interval, false otherwise
* \param from - "from" angle
* \param to - "to" angle
* \param left_limit - left limit of valid interval for angular position, left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
* \param right_limit - right limit of valid interval for angular position, left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
* \param shortest_angle - result of the shortest angle calculation
*/
static inline bool shortest_angular_distance_with_limits(double from, double to,
double left_limit, double right_limit, double &shortest_angle) {
double min_delta = -2 * M_PI;
double max_delta = 2 * M_PI;
double min_delta_to = -2 * M_PI;
double max_delta_to = 2 * M_PI;
bool flag = find_min_max_delta(from, left_limit, right_limit, min_delta,
max_delta);
double delta = shortest_angular_distance(from, to);
double delta_mod_2pi = two_pi_complement(delta);
if (flag) { //from position is within the limits
if (delta >= min_delta && delta <= max_delta) {
shortest_angle = delta;
return true;
} else if (delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta) {
shortest_angle = delta_mod_2pi;
return true;
} else { //to position is outside the limits
find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
if (fabs(min_delta_to) < fabs(max_delta_to)) {
shortest_angle = std::max<double>(delta, delta_mod_2pi);
} else if (fabs(min_delta_to) > fabs(max_delta_to)) {
shortest_angle = std::min<double>(delta, delta_mod_2pi);
} else {
if (fabs(delta) < fabs(delta_mod_2pi)) {
shortest_angle = delta;
} else {
shortest_angle = delta_mod_2pi;
}
}
return false;
}
} else { // from position is outside the limits
find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
if (fabs(min_delta) < fabs(max_delta)) {
shortest_angle = std::min<double>(delta, delta_mod_2pi);
} else if (fabs(min_delta) > fabs(max_delta)) {
shortest_angle = std::max<double>(delta, delta_mod_2pi);
} else {
if (fabs(delta) < fabs(delta_mod_2pi)) {
shortest_angle = delta;
} else {
shortest_angle = delta_mod_2pi;
}
}
return false;
}
shortest_angle = delta;
return false;
}
}// namespace math
}// namespace core
}// namespace ydlidar

302
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.cpp Normal file
View File

@@ -0,0 +1,302 @@
/*---------------------------------------------------------------------------*/
/* */
/* CActiveSocket.cpp - Active Socket Implementation */
/* */
/* Author : Mark Carrier (mark@carrierlabs.com) */
/* */
/*---------------------------------------------------------------------------*/
/* Copyright (c) 2007-2009 CarrierLabs, LLC. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* 4. The name "CarrierLabs" must not be used to
* endorse or promote products derived from this software without
* prior written permission. For written permission, please contact
* mark@carrierlabs.com.
*
* THIS SOFTWARE IS PROVIDED BY MARK CARRIER ``AS IS'' AND ANY
* EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MARK CARRIER OR
* ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (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 "ActiveSocket.h"
using namespace ydlidar;
using namespace ydlidar::core;
using namespace ydlidar::core::network;
CActiveSocket::CActiveSocket(CSocketType nType) : CSimpleSocket(nType) {
}
//------------------------------------------------------------------------------
//
// ConnectTCP() -
//
//------------------------------------------------------------------------------
bool CActiveSocket::ConnectTCP(const char *pAddr, uint16_t nPort) {
bool bRetVal = false;
struct in_addr stIpAddress;
//------------------------------------------------------------------
// Preconnection setup that must be preformed
//------------------------------------------------------------------
memset(&m_stServerSockaddr, 0, sizeof(m_stServerSockaddr));
m_stServerSockaddr.sin_family = AF_INET;
if ((m_pHE = GETHOSTBYNAME(pAddr)) == NULL) {
#if defined(_WIN32)
TranslateSocketError();
#else
if (h_errno == HOST_NOT_FOUND) {
SetSocketError(SocketInvalidAddress);
}
#endif
return bRetVal;
}
memcpy(&stIpAddress, m_pHE->h_addr_list[0], m_pHE->h_length);
m_stServerSockaddr.sin_addr.s_addr = stIpAddress.s_addr;
if ((int32_t)m_stServerSockaddr.sin_addr.s_addr == CSimpleSocket::SocketError) {
TranslateSocketError();
return bRetVal;
}
m_stServerSockaddr.sin_port = htons(nPort);
//------------------------------------------------------------------
// Connect to address "xxx.xxx.xxx.xxx" (IPv4) address only.
//
//------------------------------------------------------------------
m_timer.Initialize();
m_timer.SetStartTime();
if (connect(m_socket, (struct sockaddr *)&m_stServerSockaddr,
sizeof(m_stServerSockaddr)) ==
CSimpleSocket::SocketError) {
//--------------------------------------------------------------
// Get error value this might be a non-blocking socket so we
// must first check.
//--------------------------------------------------------------
TranslateSocketError();
//--------------------------------------------------------------
// If the socket is non-blocking and the current socket error
// is SocketEinprogress or SocketEwouldblock then poll connection
// with select for designated timeout period.
// Linux returns EINPROGRESS and Windows returns WSAEWOULDBLOCK.
//--------------------------------------------------------------
if ((IsNonblocking()) &&
((GetSocketError() == CSimpleSocket::SocketEwouldblock) ||
(GetSocketError() == CSimpleSocket::SocketEinprogress))) {
bRetVal = Select(GetConnectTimeoutSec(), GetConnectTimeoutUSec());
}
} else {
TranslateSocketError();
bRetVal = true;
}
m_timer.SetEndTime();
return bRetVal;
}
//------------------------------------------------------------------------------
//
// ConnectUDP() -
//
//------------------------------------------------------------------------------
bool CActiveSocket::ConnectUDP(const char *pAddr, uint16_t nPort) {
bool bRetVal = false;
struct in_addr stIpAddress;
//------------------------------------------------------------------
// Pre-connection setup that must be preformed
//------------------------------------------------------------------
memset(&m_stServerSockaddr, 0, sizeof(m_stServerSockaddr));
m_stServerSockaddr.sin_family = AF_INET;
if ((m_pHE = GETHOSTBYNAME(pAddr)) == NULL) {
#if defined(_WIN32)
TranslateSocketError();
#else
if (h_errno == HOST_NOT_FOUND) {
SetSocketError(SocketInvalidAddress);
}
#endif
return bRetVal;
}
memcpy(&stIpAddress, m_pHE->h_addr_list[0], m_pHE->h_length);
m_stServerSockaddr.sin_addr.s_addr = stIpAddress.s_addr;
if ((int32_t)m_stServerSockaddr.sin_addr.s_addr == CSimpleSocket::SocketError) {
TranslateSocketError();
return bRetVal;
}
m_stServerSockaddr.sin_port = htons(nPort);
//------------------------------------------------------------------
// Connect to address "xxx.xxx.xxx.xxx" (IPv4) address only.
//
//------------------------------------------------------------------
m_timer.Initialize();
m_timer.SetStartTime();
if (connect(m_socket, (struct sockaddr *)&m_stServerSockaddr,
sizeof(m_stServerSockaddr)) != CSimpleSocket::SocketError) {
bRetVal = true;
}
TranslateSocketError();
m_timer.SetEndTime();
return bRetVal;
}
//------------------------------------------------------------------------------
//
// ConnectRAW() -
//
//------------------------------------------------------------------------------
bool CActiveSocket::ConnectRAW(const char *pAddr, uint16_t nPort) {
bool bRetVal = false;
struct in_addr stIpAddress;
//------------------------------------------------------------------
// Pre-connection setup that must be preformed
//------------------------------------------------------------------
memset(&m_stServerSockaddr, 0, sizeof(m_stServerSockaddr));
m_stServerSockaddr.sin_family = AF_INET;
if ((m_pHE = GETHOSTBYNAME(pAddr)) == NULL) {
#if defined(_WIN32)
TranslateSocketError();
#else
if (h_errno == HOST_NOT_FOUND) {
SetSocketError(SocketInvalidAddress);
}
#endif
return bRetVal;
}
memcpy(&stIpAddress, m_pHE->h_addr_list[0], m_pHE->h_length);
m_stServerSockaddr.sin_addr.s_addr = stIpAddress.s_addr;
if ((int32_t)m_stServerSockaddr.sin_addr.s_addr == CSimpleSocket::SocketError) {
TranslateSocketError();
return bRetVal;
}
m_stServerSockaddr.sin_port = htons(nPort);
//------------------------------------------------------------------
// Connect to address "xxx.xxx.xxx.xxx" (IPv4) address only.
//
//------------------------------------------------------------------
m_timer.Initialize();
m_timer.SetStartTime();
if (connect(m_socket, (struct sockaddr *)&m_stServerSockaddr,
sizeof(m_stServerSockaddr)) != CSimpleSocket::SocketError) {
bRetVal = true;
}
TranslateSocketError();
m_timer.SetEndTime();
return bRetVal;
}
//------------------------------------------------------------------------------
//
// Open() - Create a connection to a specified address on a specified port
//
//------------------------------------------------------------------------------
bool CActiveSocket::Open(const char *pAddr, uint16_t nPort) {
bool bRetVal = false;
if (IsSocketValid() == false) {
SetSocketError(CSimpleSocket::SocketInvalidSocket);
return bRetVal;
}
if (pAddr == NULL) {
SetSocketError(CSimpleSocket::SocketInvalidAddress);
return bRetVal;
}
if (nPort == 0) {
SetSocketError(CSimpleSocket::SocketInvalidPort);
return bRetVal;
}
switch (m_nSocketType) {
case CSimpleSocket::SocketTypeTcp : {
bRetVal = ConnectTCP(pAddr, nPort);
break;
}
case CSimpleSocket::SocketTypeUdp : {
bRetVal = ConnectUDP(pAddr, nPort);
break;
}
case CSimpleSocket::SocketTypeRaw :
break;
default:
break;
}
//--------------------------------------------------------------------------
// If successful then create a local copy of the address and port
//--------------------------------------------------------------------------
if (bRetVal) {
socklen_t nSockLen = sizeof(struct sockaddr);
memset(&m_stServerSockaddr, 0, nSockLen);
getpeername(m_socket, (struct sockaddr *)&m_stServerSockaddr, &nSockLen);
nSockLen = sizeof(struct sockaddr);
memset(&m_stClientSockaddr, 0, nSockLen);
getsockname(m_socket, (struct sockaddr *)&m_stClientSockaddr, &nSockLen);
SetSocketError(SocketSuccess);
}
return bRetVal;
}

100
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.h Normal file
View File

@@ -0,0 +1,100 @@
/*---------------------------------------------------------------------------*/
/* */
/* ActiveSocket.h - Active Socket Decleration */
/* */
/* Author : Mark Carrier (mark@carrierlabs.com) */
/* */
/*---------------------------------------------------------------------------*/
/* Copyright (c) 2007-2009 CarrierLabs, LLC. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* 4. The name "CarrierLabs" must not be used to
* endorse or promote products derived from this software without
* prior written permission. For written permission, please contact
* mark@carrierlabs.com.
*
* THIS SOFTWARE IS PROVIDED BY MARK CARRIER ``AS IS'' AND ANY
* EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MARK CARRIER OR
* ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
* OF THE POSSIBILITY OF SUCH DAMAGE.
*----------------------------------------------------------------------------*/
#ifndef __ACTIVESOCKET_H__
#define __ACTIVESOCKET_H__
#include "SimpleSocket.h"
namespace ydlidar {
namespace core {
namespace network {
class CPassiveSocket;
/// Provides a platform independent class to create an active socket.
/// An active socket is used to create a socket which connects to a server.
/// This type of object would be used when an application needs to send/receive
/// data from a server.
class CActiveSocket : public CSimpleSocket {
public:
friend class CPassiveSocket;
explicit CActiveSocket(CSocketType type = SocketTypeTcp);
virtual ~CActiveSocket() {
Close();
};
/// Established a connection to the address specified by pAddr.
/// Connection-based protocol sockets (CSocket::SocketTypeTcp) may
/// successfully call open() only once, however; connectionless protocol
/// sockets (CSocket::SocketTypeUdp) may use Open() multiple times to
/// change their association.
/// @param pAddr specifies the destination address to connect.
/// @param nPort specifies the destination port.
/// @return true if successful connection made, otherwise false.
virtual bool Open(const char *pAddr, uint16_t nPort);
private:
/// Utility function used to create a TCP connection, called from Open().
/// @return true if successful connection made, otherwise false.
bool ConnectTCP(const char *pAddr, uint16_t nPort);
/// Utility function used to create a UDP connection, called from Open().
/// @return true if successful connection made, otherwise false.
bool ConnectUDP(const char *pAddr, uint16_t nPort);
/// Utility function used to create a RAW connection, called from Open().
/// @return true if successful connection made, otherwise false.
bool ConnectRAW(const char *pAddr, uint16_t nPort);
private:
struct hostent *m_pHE;
};
}//namespace socket
}//namespace core
}//namespace ydlidar
#endif /* __ACTIVESOCKET_H__ */

10
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/CMakeLists.txt Normal file
View File

@@ -0,0 +1,10 @@
aux_include_directory(. HDRS)
aux_source_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})
IF (WIN32)
add_to_ydlidar_libraries(setupapi ws2_32)
ELSE()
add_to_ydlidar_libraries(rt)
ENDIF()

315
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.cpp Normal file
View File

@@ -0,0 +1,315 @@
/*---------------------------------------------------------------------------*/
/* */
/* PassiveSocket.cpp - Passive Socket Implementation */
/* */
/* Author : Mark Carrier (mark@carrierlabs.com) */
/* */
/*---------------------------------------------------------------------------*/
/* Copyright (c) 2007-2009 CarrierLabs, LLC. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* 4. The name "CarrierLabs" must not be used to
* endorse or promote products derived from this software without
* prior written permission. For written permission, please contact
* mark@carrierlabs.com.
*
* THIS SOFTWARE IS PROVIDED BY MARK CARRIER ``AS IS'' AND ANY
* EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MARK CARRIER OR
* ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (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 "PassiveSocket.h"
using namespace ydlidar;
using namespace ydlidar::core;
using namespace ydlidar::core::network;
CPassiveSocket::CPassiveSocket(CSocketType nType) : CSimpleSocket(nType) {
}
bool CPassiveSocket::BindMulticast(const char *pInterface, const char *pGroup,
uint16_t nPort) {
bool bRetVal = false;
#if defined(_WIN32)
ULONG inAddr;
#else
int32_t nReuse;
in_addr_t inAddr;
nReuse = IPTOS_LOWDELAY;
#endif
//--------------------------------------------------------------------------
// Set the following socket option SO_REUSEADDR. This will allow the file
// descriptor to be reused immediately after the socket is closed instead
// of setting in a TIMED_WAIT state.
//--------------------------------------------------------------------------
memset(&m_stMulticastGroup, 0, sizeof(m_stMulticastGroup));
m_stMulticastGroup.sin_family = AF_INET;
m_stMulticastGroup.sin_port = htons(nPort);
//--------------------------------------------------------------------------
// If no IP Address (interface ethn) is supplied, or the loop back is
// specified then bind to any interface, else bind to specified interface.
//--------------------------------------------------------------------------
if ((pInterface == NULL) || (!strlen(pInterface))) {
m_stMulticastGroup.sin_addr.s_addr = htonl(INADDR_ANY);
} else {
if ((inAddr = inet_addr(pInterface)) != INADDR_NONE) {
m_stMulticastGroup.sin_addr.s_addr = inAddr;
}
}
//--------------------------------------------------------------------------
// Bind to the specified port
//--------------------------------------------------------------------------
if (bind(m_socket, (struct sockaddr *)&m_stMulticastGroup,
sizeof(m_stMulticastGroup)) == 0) {
//----------------------------------------------------------------------
// Join the multicast group
//----------------------------------------------------------------------
m_stMulticastRequest.imr_multiaddr.s_addr = inet_addr(pGroup);
m_stMulticastRequest.imr_interface.s_addr = m_stMulticastGroup.sin_addr.s_addr;
if (SETSOCKOPT(m_socket, IPPROTO_IP, IP_ADD_MEMBERSHIP,
(void *)&m_stMulticastRequest,
sizeof(m_stMulticastRequest)) == CSimpleSocket::SocketSuccess) {
bRetVal = true;
}
m_timer.SetEndTime();
}
m_timer.Initialize();
m_timer.SetStartTime();
//--------------------------------------------------------------------------
// If there was a socket error then close the socket to clean out the
// connection in the backlog.
//--------------------------------------------------------------------------
TranslateSocketError();
if (bRetVal == false) {
Close();
}
return bRetVal;
}
//------------------------------------------------------------------------------
//
// Listen() -
//
//------------------------------------------------------------------------------
bool CPassiveSocket::Listen(const char *pAddr, uint16_t nPort,
int32_t nConnectionBacklog) {
bool bRetVal = false;
#if defined(_WIN32)
ULONG inAddr;
#else
int32_t nReuse;
in_addr_t inAddr;
nReuse = IPTOS_LOWDELAY;
#endif
//--------------------------------------------------------------------------
// Set the following socket option SO_REUSEADDR. This will allow the file
// descriptor to be reused immediately after the socket is closed instead
// of setting in a TIMED_WAIT state.
//--------------------------------------------------------------------------
#if defined(__linux__)
SETSOCKOPT(m_socket, SOL_SOCKET, SO_REUSEADDR, (char *)&nReuse,
sizeof(int32_t));
SETSOCKOPT(m_socket, IPPROTO_TCP, IP_TOS, &nReuse, sizeof(int32_t));
#endif
memset(&m_stServerSockaddr, 0, sizeof(m_stServerSockaddr));
m_stServerSockaddr.sin_family = AF_INET;
m_stServerSockaddr.sin_port = htons(nPort);
//--------------------------------------------------------------------------
// If no IP Address (interface ethn) is supplied, or the loop back is
// specified then bind to any interface, else bind to specified interface.
//--------------------------------------------------------------------------
if ((pAddr == NULL) || (!strlen(pAddr))) {
m_stServerSockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
} else {
if ((inAddr = inet_addr(pAddr)) != INADDR_NONE) {
m_stServerSockaddr.sin_addr.s_addr = inAddr;
}
}
m_timer.Initialize();
m_timer.SetStartTime();
//--------------------------------------------------------------------------
// Bind to the specified port
//--------------------------------------------------------------------------
if (bind(m_socket, (struct sockaddr *)&m_stServerSockaddr,
sizeof(m_stServerSockaddr)) != CSimpleSocket::SocketError) {
if (m_nSocketType == CSimpleSocket::SocketTypeTcp) {
if (listen(m_socket, nConnectionBacklog) != CSimpleSocket::SocketError) {
bRetVal = true;
}
} else {
bRetVal = true;
}
}
m_timer.SetEndTime();
//--------------------------------------------------------------------------
// If there was a socket error then close the socket to clean out the
// connection in the backlog.
//--------------------------------------------------------------------------
TranslateSocketError();
if (bRetVal == false) {
CSocketError err = GetSocketError();
Close();
SetSocketError(err);
}
return bRetVal;
}
//------------------------------------------------------------------------------
//
// Accept() -
//
//------------------------------------------------------------------------------
CActiveSocket *CPassiveSocket::Accept() {
uint32_t nSockLen;
CActiveSocket *pClientSocket = NULL;
SOCKET socket = CSimpleSocket::SocketError;
if (m_nSocketType != CSimpleSocket::SocketTypeTcp) {
SetSocketError(CSimpleSocket::SocketProtocolError);
return pClientSocket;
}
pClientSocket = new CActiveSocket();
//--------------------------------------------------------------------------
// Wait for incoming connection.
//--------------------------------------------------------------------------
if (pClientSocket != NULL) {
CSocketError socketErrno = SocketSuccess;
m_timer.Initialize();
m_timer.SetStartTime();
nSockLen = sizeof(m_stClientSockaddr);
do {
errno = 0;
socket = accept(m_socket, (struct sockaddr *)&m_stClientSockaddr,
(socklen_t *)&nSockLen);
if (socket != -1) {
pClientSocket->SetSocketHandle(socket);
pClientSocket->TranslateSocketError();
socketErrno = pClientSocket->GetSocketError();
socklen_t nSockLen = sizeof(struct sockaddr);
//-------------------------------------------------------------
// Store client and server IP and port information for this
// connection.
//-------------------------------------------------------------
getpeername(m_socket, (struct sockaddr *)&pClientSocket->m_stClientSockaddr,
&nSockLen);
memcpy((void *)&pClientSocket->m_stClientSockaddr, (void *)&m_stClientSockaddr,
nSockLen);
memset(&pClientSocket->m_stServerSockaddr, 0, nSockLen);
getsockname(m_socket, (struct sockaddr *)&pClientSocket->m_stServerSockaddr,
&nSockLen);
} else {
TranslateSocketError();
socketErrno = GetSocketError();
}
} while (socketErrno == CSimpleSocket::SocketInterrupted);
m_timer.SetEndTime();
if (socketErrno != CSimpleSocket::SocketSuccess) {
delete pClientSocket;
pClientSocket = NULL;
}
}
return pClientSocket;
}
//------------------------------------------------------------------------------
//
// Send() - Send data on a valid socket
//
//------------------------------------------------------------------------------
int32_t CPassiveSocket::Send(const uint8_t *pBuf, size_t bytesToSend) {
SetSocketError(SocketSuccess);
m_nBytesSent = 0;
switch (m_nSocketType) {
case CSimpleSocket::SocketTypeUdp: {
if (IsSocketValid()) {
if ((bytesToSend > 0) && (pBuf != NULL)) {
m_timer.Initialize();
m_timer.SetStartTime();
m_nBytesSent = SENDTO(m_socket, pBuf, bytesToSend, 0,
(const sockaddr *)&m_stClientSockaddr,
sizeof(m_stClientSockaddr));
m_timer.SetEndTime();
if (m_nBytesSent == CSimpleSocket::SocketError) {
TranslateSocketError();
}
}
}
break;
}
case CSimpleSocket::SocketTypeTcp:
CSimpleSocket::Send(pBuf, bytesToSend);
break;
default:
SetSocketError(SocketProtocolError);
break;
}
return m_nBytesSent;
}

128
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.h Normal file
View File

@@ -0,0 +1,128 @@
/*---------------------------------------------------------------------------*/
/* */
/* Socket.h - Passive Socket Decleration. */
/* */
/* Author : Mark Carrier (mark@carrierlabs.com) */
/* */
/*---------------------------------------------------------------------------*/
/* Copyright (c) 2007-2009 CarrierLabs, LLC. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* 4. The name "CarrierLabs" must not be used to
* endorse or promote products derived from this software without
* prior written permission. For written permission, please contact
* mark@carrierlabs.com.
*
* THIS SOFTWARE IS PROVIDED BY MARK CARRIER ``AS IS'' AND ANY
* EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MARK CARRIER OR
* ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
* OF THE POSSIBILITY OF SUCH DAMAGE.
*----------------------------------------------------------------------------*/
#ifndef __PASSIVESOCKET_H__
#define __PASSIVESOCKET_H__
#include "ActiveSocket.h"
/// Provides a platform independent class to create a passive socket.
/// A passive socket is used to create a "listening" socket. This type
/// of object would be used when an application needs to wait for
/// inbound connections. Support for CSimpleSocket::SocketTypeTcp,
/// CSimpleSocket::SocketTypeUdp, and CSimpleSocket::SocketTypeRaw is handled
/// in a similar fashion. The big difference is that the method
/// CPassiveSocket::Accept should not be called on the latter two socket
/// types.
///
namespace ydlidar {
namespace core {
namespace network {
class CPassiveSocket : public CSimpleSocket {
public:
explicit CPassiveSocket(CSocketType type = SocketTypeTcp);
virtual ~CPassiveSocket() {
Close();
};
/// Extracts the first connection request on the queue of pending
/// connections and creates a newly connected socket. Used with
/// CSocketType CSimpleSocket::SocketTypeTcp. It is the responsibility of
/// the caller to delete the returned object when finished.
/// @return if successful a pointer to a newly created CActiveSocket object
/// will be returned and the internal error condition of the CPassiveSocket
/// object will be CPassiveSocket::SocketSuccess. If an error condition was encountered
/// the NULL will be returned and one of the following error conditions will be set:
/// CPassiveSocket::SocketEwouldblock, CPassiveSocket::SocketInvalidSocket,
/// CPassiveSocket::SocketConnectionAborted, CPassiveSocket::SocketInterrupted
/// CPassiveSocket::SocketProtocolError, CPassiveSocket::SocketFirewallError
virtual CActiveSocket *Accept(void);
/// Bind to a multicast group on a specified interface, multicast group, and port
///
/// @param pInterface - interface on which to bind.
/// @param pGroup - multicast group address to bind.
/// @param nPort - port on which multicast
/// @return true if able to bind to interface and multicast group.
/// If not successful, the false is returned and one of the following error
/// condiitions will be set: CPassiveSocket::SocketAddressInUse, CPassiveSocket::SocketProtocolError,
/// CPassiveSocket::SocketInvalidSocket. The following socket errors are for Linux/Unix
/// derived systems only: CPassiveSocket::SocketInvalidSocketBuffer
bool BindMulticast(const char *pInterface, const char *pGroup, uint16_t nPort);
/// Create a listening socket at local ip address 'x.x.x.x' or 'localhost'
/// if pAddr is NULL on port nPort.
///
/// @param pAddr specifies the IP address on which to listen.
/// @param nPort specifies the port on which to listen.
/// @param nConnectionBacklog specifies connection queue backlog (default 30,000)
/// @return true if a listening socket was created.
/// If not successful, the false is returned and one of the following error
/// conditions will be set: CPassiveSocket::SocketAddressInUse, CPassiveSocket::SocketProtocolError,
/// CPassiveSocket::SocketInvalidSocket. The following socket errors are for Linux/Unix
/// derived systems only: CPassiveSocket::SocketInvalidSocketBuffer
virtual bool Listen(const char *pAddr, uint16_t nPort,
int32_t nConnectionBacklog = 30000);
/// Attempts to send a block of data on an established connection.
/// @param pBuf block of data to be sent.
/// @param bytesToSend size of data block to be sent.
/// @return number of bytes actually sent, return of zero means the
/// connection has been shutdown on the other side, and a return of -1
/// means that an error has occurred. If an error was signaled then one
/// of the following error codes will be set: CPassiveSocket::SocketInvalidSocket,
/// CPassiveSocket::SocketEwouldblock, SimpleSocket::SocketConnectionReset,
/// CPassiveSocket::SocketInvalidSocketBuffer, CPassiveSocket::SocketInterrupted,
/// CPassiveSocket::SocketProtocolError, CPassiveSocket::SocketNotconnected
/// <br>\b Note: This function is used only for a socket of type
/// CSimpleSocket::SocketTypeUdp
virtual int32_t Send(const uint8_t *pBuf, size_t bytesToSend);
private:
struct ip_mreq m_stMulticastRequest; /// group address for multicast
};
}//namespace socket
}//namespace core
}// namespace ydlidar
#endif // __PASSIVESOCKET_H__

1514
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

691
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.h Normal file
View File

@@ -0,0 +1,691 @@
/*---------------------------------------------------------------------------*/
/* */
/* SimpleSocket.h - Simple Socket base class decleration. */
/* */
/* Author : Mark Carrier (mark@carrierlabs.com) */
/* */
/*---------------------------------------------------------------------------*/
/* Copyright (c) 2007-2009 CarrierLabs, LLC. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* 4. The name "CarrierLabs" must not be used to
* endorse or promote products derived from this software without
* prior written permission. For written permission, please contact
* mark@carrierlabs.com.
*
* THIS SOFTWARE IS PROVIDED BY MARK CARRIER ``AS IS'' AND ANY
* EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MARK CARRIER OR
* ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
* OF THE POSSIBILITY OF SUCH DAMAGE.
*----------------------------------------------------------------------------*/
#ifndef __SOCKET_H__
#define __SOCKET_H__
#include <sys/stat.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <errno.h>
#include <assert.h>
#include <thread>
#include <chrono>
#if defined(__linux__) || defined (_DARWIN)
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <netinet/ip.h>
#include <netdb.h>
#endif
#ifdef __linux__
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <linux/if.h>
#include <sys/sendfile.h>
#endif
#ifdef _DARWIN
#include <net/if.h>
#endif
#if defined(__linux__) || defined (_DARWIN)
#include <sys/time.h>
#include <sys/uio.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#endif
#if defined(_WIN32)
#include <io.h>
#include <winsock2.h>
#include <Ws2tcpip.h>
#pragma warning(disable: 4786)
#pragma comment(lib, "ws2_32.lib")
#pragma comment(lib, "setupapi.lib")
#define IPTOS_LOWDELAY 0x10
#endif
#include "StatTimer.h"
#include <core/common/ChannelDevice.h>
//-----------------------------------------------------------------------------
// General class macro definitions and typedefs
//-----------------------------------------------------------------------------
#ifndef INVALID_SOCKET
#define INVALID_SOCKET ~(0)
#endif
#define SOCKET_SENDFILE_BLOCKSIZE 8192
namespace ydlidar {
namespace core {
using namespace common;
namespace network {
/// Provides a platform independent class to for socket development.
/// This class is designed to abstract socket communication development in a
/// platform independent manner.
/// - Socket types
/// -# CActiveSocket Class
/// -# CPassiveSocket Class
class CSimpleSocket : public ChannelDevice {
public:
/// Defines the three possible states for shuting down a socket.
typedef enum {
Receives = SHUT_RD, ///< Shutdown passive socket.
Sends = SHUT_WR, ///< Shutdown active socket.
Both = SHUT_RDWR ///< Shutdown both active and passive sockets.
} CShutdownMode;
/// Defines the socket types defined by CSimpleSocket class.
typedef enum {
SocketTypeInvalid = 0, ///< Invalid socket type.
SocketTypeTcp, ///< Defines socket as TCP socket.
SocketTypeUdp, ///< Defines socket as UDP socket.
SocketTypeTcp6, ///< Defines socket as IPv6 TCP socket.
SocketTypeUdp6, ///< Defines socket as IPv6 UDP socket.
SocketTypeRaw ///< Provides raw network protocol access.
} CSocketType;
/// Defines all error codes handled by the CSimpleSocket class.
typedef enum {
SocketError = -1, ///< Generic socket error translates to error below.
SocketSuccess = 0, ///< No socket error.
SocketInvalidSocket, ///< Invalid socket handle.
SocketInvalidAddress, ///< Invalid destination address specified.
SocketInvalidPort, ///< Invalid destination port specified.
SocketConnectionRefused, ///< No server is listening at remote address.
SocketTimedout, ///< Timed out while attempting operation.
SocketEwouldblock, ///< Operation would block if socket were blocking.
SocketNotconnected, ///< Currently not connected.
SocketEinprogress, ///< Socket is non-blocking and the connection cannot be completed immediately
SocketInterrupted, ///< Call was interrupted by a signal that was caught before a valid connection arrived.
SocketConnectionAborted, ///< The connection has been aborted.
SocketProtocolError, ///< Invalid protocol for operation.
SocketFirewallError, ///< Firewall rules forbid connection.
SocketInvalidSocketBuffer, ///< The receive buffer point outside the process's address space.
SocketConnectionReset, ///< Connection was forcibly closed by the remote host.
SocketAddressInUse, ///< Address already in use.
SocketInvalidPointer, ///< Pointer type supplied as argument is invalid.
SocketEunknown ///< Unknown error please report to mark@carrierlabs.com
} CSocketError;
public:
explicit CSimpleSocket(CSocketType type = SocketTypeTcp);
explicit CSimpleSocket(CSimpleSocket &socket);
virtual ~CSimpleSocket() {
if (m_pBuffer != NULL) {
delete [] m_pBuffer;
m_pBuffer = NULL;
}
};
static void WSACleanUp();
/// Initialize instance of CSocket. This method MUST be called before an
/// object can be used. Errors : CSocket::SocketProtocolError,
/// CSocket::SocketInvalidSocket,
/// @return true if properly initialized.
virtual bool Initialize(void);
/// Close socket
/// @return true if successfully closed otherwise returns false.
virtual bool Close(void);
/// Shutdown shut down socket send and receive operations
/// CShutdownMode::Receives - Disables further receive operations.
/// CShutdownMode::Sends - Disables further send operations.
/// CShutdownBoth:: - Disables further send and receive operations.
/// @param nShutdown specifies the type of shutdown.
/// @return true if successfully shutdown otherwise returns false.
virtual bool Shutdown(CShutdownMode nShutdown);
/// Examine the socket descriptor sets currently owned by the instance of
/// the socket class (the readfds, writefds, and errorfds parameters) to
/// see whether some of their descriptors are ready for reading, are ready
/// for writing, or have an exceptional condition pending, respectively.
/// Block until an event happens on the specified file descriptors.
/// @return true if socket has data ready, or false if not ready or timed out.
virtual bool Select(void) {
return Select(0, 0);
};
/// Examine the socket descriptor sets currently owned by the instance of
/// the socket class (the readfds, writefds, and errorfds parameters) to
/// see whether some of their descriptors are ready for reading, are ready
/// for writing, or have an exceptional condition pending, respectively.
/// @param nTimeoutSec timeout in seconds for select.
/// @param nTimeoutUSec timeout in micro seconds for select.
/// @return true if socket has data ready, or false if not ready or timed out.
virtual bool Select(int32_t nTimeoutSec, int32_t nTimeoutUSec);
virtual int WaitForData(size_t data_count, uint32_t timeout,
size_t *returned_size);
/// Does the current instance of the socket object contain a valid socket
/// descriptor.
/// @return true if the socket object contains a valid socket descriptor.
virtual bool IsSocketValid(void) {
return (m_socket != SocketError);
};
/// Provides a standard error code for cross platform development by
/// mapping the operating system error to an error defined by the CSocket
/// class.
void TranslateSocketError(void);
/// Returns a human-readable description of the given error code
/// or the last error code of a socket
static const char *DescribeError(CSocketError err);
///
/// \brief DescribeError
/// \return
///
virtual const char *DescribeError() {
return DescribeError(m_socketErrno);
};
/// Attempts to receive a block of data on an established connection.
/// @param nMaxBytes maximum number of bytes to receive.
/// @param pBuffer, memory where to receive the data,
/// NULL receives to internal buffer returned with GetData()
/// Non-NULL receives directly there, but GetData() will return WRONG ptr!
/// @return number of bytes actually received.
/// @return of zero means the connection has been shutdown on the other side.
/// @return of -1 means that an error has occurred.
virtual int32_t Receive(int32_t nMaxBytes = 1, uint8_t *pBuffer = 0);
/// Attempts to send a block of data on an established connection.
/// @param pBuf block of data to be sent.
/// @param bytesToSend size of data block to be sent.
/// @return number of bytes actually sent.
/// @return of zero means the connection has been shutdown on the other side.
/// @return of -1 means that an error has occurred.
virtual int32_t Send(const uint8_t *pBuf, size_t bytesToSend);
/// Attempts to send at most nNumItem blocks described by sendVector
/// to the socket descriptor associated with the socket object.
/// @param sendVector pointer to an array of iovec structures
/// @param nNumItems number of items in the vector to process
/// <br>\b NOTE: Buffers are processed in the order specified.
/// @return number of bytes actually sent, return of zero means the
/// connection has been shutdown on the other side, and a return of -1
/// means that an error has occurred.
virtual int32_t Send(const struct iovec *sendVector, int32_t nNumItems);
/// Copies data between one file descriptor and another.
/// On some systems this copying is done within the kernel, and thus is
/// more efficient than the combination of CSimpleSocket::Send and
/// CSimpleSocket::Receive, which would require transferring data to and
/// from user space.
/// <br>\b Note: This is available on all implementations, but the kernel
/// implementation is only available on Unix type systems.
/// @param nOutFd descriptor opened for writing.
/// @param nInFd descriptor opened for reading.
/// @param pOffset from which to start reading data from input file.
/// @param nCount number of bytes to copy between file descriptors.
/// @return number of bytes written to the out socket descriptor.
virtual int32_t SendFile(int32_t nOutFd, int32_t nInFd, off_t *pOffset,
int32_t nCount);
/// Returns blocking/non-blocking state of socket.
/// @return true if the socket is non-blocking, else return false.
bool IsNonblocking(void) {
return (m_bIsBlocking == false);
};
/// Set the socket to blocking.
/// @return true if successful set to blocking, else return false;
bool SetBlocking(void);
/// Set the socket as non-blocking.
/// @return true if successful set to non-blocking, else return false;
bool SetNonblocking(void);
/// Get a pointer to internal receive buffer. The user MUST not free this
/// pointer when finished. This memory is managed internally by the CSocket
/// class.
/// @return pointer to data if valid, else returns NULL.
uint8_t *GetData(void) {
return m_pBuffer;
};
/// Returns the number of bytes received on the last call to
/// CSocket::Receive().
/// @return number of bytes received.
int32_t GetBytesReceived(void) {
return m_nBytesReceived;
};
/// Returns the number of bytes sent on the last call to
/// CSocket::Send().
/// @return number of bytes sent.
int32_t GetBytesSent(void) {
return m_nBytesSent;
};
/// Controls the actions taken when CSimpleSocket::Close is executed on a
/// socket object that has unsent data. The default value for this option
/// is \b off.
/// - Following are the three possible scenarios.
/// -# \b bEnable is false, CSimpleSocket::Close returns immediately, but
/// any unset data is transmitted (after CSimpleSocket::Close returns)
/// -# \b bEnable is true and \b nTime is zero, CSimpleSocket::Close return
/// immediately and any unsent data is discarded.
/// -# \b bEnable is true and \b nTime is nonzero, CSimpleSocket::Close does
/// not return until all unsent data is transmitted (or the connection is
/// Closed by the remote system).
/// <br><p>
/// @param bEnable true to enable option false to disable option.
/// @param nTime time in seconds to linger.
/// @return true if option successfully set
bool SetOptionLinger(bool bEnable, uint16_t nTime);
/// Tells the kernel that even if this port is busy (in the TIME_WAIT state),
/// go ahead and reuse it anyway. If it is busy, but with another state,
/// you will still get an address already in use error.
/// @return true if option successfully set
bool SetOptionReuseAddr();
/// Gets the timeout value that specifies the maximum number of seconds a
/// call to CSimpleSocket::Open waits until it completes.
/// @return the length of time in seconds
int32_t GetConnectTimeoutSec(void) {
return m_stConnectTimeout.tv_sec;
};
/// Gets the timeout value that specifies the maximum number of microseconds
/// a call to CSimpleSocket::Open waits until it completes.
/// @return the length of time in microseconds
int32_t GetConnectTimeoutUSec(void) {
return m_stConnectTimeout.tv_usec;
};
/// Sets the timeout value that specifies the maximum amount of time a call
/// to CSimpleSocket::Receive waits until it completes. Use the method
/// CSimpleSocket::SetReceiveTimeout to specify the number of seconds to wait.
/// If a call to CSimpleSocket::Receive has blocked for the specified length of
/// time without receiving additional data, it returns with a partial count
/// or CSimpleSocket::GetSocketError set to CSimpleSocket::SocketEwouldblock if no data
/// were received.
/// @param nConnectTimeoutSec of timeout in seconds.
/// @param nConnectTimeoutUsec of timeout in microseconds.
/// @return true if socket connection timeout was successfully set.
void SetConnectTimeout(int32_t nConnectTimeoutSec,
int32_t nConnectTimeoutUsec = 0) {
m_stConnectTimeout.tv_sec = nConnectTimeoutSec;
m_stConnectTimeout.tv_usec = nConnectTimeoutUsec;
};
/// Gets the timeout value that specifies the maximum number of seconds a
/// a call to CSimpleSocket::Receive waits until it completes.
/// @return the length of time in seconds
int32_t GetReceiveTimeoutSec(void) {
return m_stRecvTimeout.tv_sec;
};
/// Gets the timeout value that specifies the maximum number of microseconds
/// a call to CSimpleSocket::Receive waits until it completes.
/// @return the length of time in microseconds
int32_t GetReceiveTimeoutUSec(void) {
return m_stRecvTimeout.tv_usec;
};
/// Sets the timeout value that specifies the maximum amount of time a call
/// to CSimpleSocket::Receive waits until it completes. Use the method
/// CSimpleSocket::SetReceiveTimeout to specify the number of seconds to wait.
/// If a call to CSimpleSocket::Receive has blocked for the specified length of
/// time without receiving additional data, it returns with a partial count
/// or CSimpleSocket::GetSocketError set to CSimpleSocket::SocketEwouldblock if no data
/// were received.
/// @param nRecvTimeoutSec of timeout in seconds.
/// @param nRecvTimeoutUsec of timeout in microseconds.
/// @return true if socket timeout was successfully set.
bool SetReceiveTimeout(int32_t nRecvTimeoutSec, int32_t nRecvTimeoutUsec = 0);
/// Enable/disable multicast for a socket. This options is only valid for
/// socket descriptors of type CSimpleSocket::SocketTypeUdp.
/// @return true if multicast was enabled or false if socket type is not
/// CSimpleSocket::SocketTypeUdp and the error will be set to
/// CSimpleSocket::SocketProtocolError
bool SetMulticast(bool bEnable, uint8_t multicastTTL = 1);
/// Return true if socket is multicast or false is socket is unicast
/// @return true if multicast is enabled
bool GetMulticast() {
return m_bIsMulticast;
};
/// Bind socket to a specific interface when using multicast.
/// @return true if successfully bound to interface
bool BindInterface(const char *pInterface);
/// Gets the timeout value that specifies the maximum number of seconds a
/// a call to CSimpleSocket::Send waits until it completes.
/// @return the length of time in seconds
int32_t GetSendTimeoutSec(void) {
return m_stSendTimeout.tv_sec;
};
/// Gets the timeout value that specifies the maximum number of microseconds
/// a call to CSimpleSocket::Send waits until it completes.
/// @return the length of time in microseconds
int32_t GetSendTimeoutUSec(void) {
return m_stSendTimeout.tv_usec;
};
/// Gets the timeout value that specifies the maximum amount of time a call
/// to CSimpleSocket::Send waits until it completes.
/// @return the length of time in seconds
bool SetSendTimeout(int32_t nSendTimeoutSec, int32_t nSendTimeoutUsec = 0);
/// Returns the last error that occured for the instace of the CSimpleSocket
/// instance. This method should be called immediately to retrieve the
/// error code for the failing mehtod call.
/// @return last error that occured.
CSocketError GetSocketError(void) {
return m_socketErrno;
};
/// Get the total time the of the last operation in milliseconds.
/// @return number of milliseconds of last operation.
uint32_t GetTotalTimeMs() {
return m_timer.GetMilliSeconds();
};
/// Get the total time the of the last operation in microseconds.
/// @return number of microseconds or last operation.
uint64_t GetTotalTimeUsec() {
return m_timer.GetMicroSeconds();
};
/// Return Differentiated Services Code Point (DSCP) value currently set on the socket object.
/// @return DSCP for current socket object.
/// <br><br> \b NOTE: Windows special notes http://support.microsoft.com/kb/248611.
int GetSocketDscp(void);
/// Set Differentiated Services Code Point (DSCP) for socket object.
/// @param nDscp value of TOS setting which will be converted to DSCP
/// @return true if DSCP value was properly set
/// <br><br> \b NOTE: Windows special notes http://support.microsoft.com/kb/248611.
bool SetSocketDscp(int nDscp);
/// Return socket descriptor
/// @return socket descriptor which is a signed 32 bit integer.
SOCKET GetSocketDescriptor() {
return m_socket;
};
/// Return socket descriptor
/// @return socket descriptor which is a signed 32 bit integer.
CSocketType GetSocketType() {
return m_nSocketType;
};
/// set socket descriptor
void SetSocketType(const CSocketType &type) {
m_nSocketType = type;
}
/// Returns clients Internet host address as a string in standard numbers-and-dots notation.
/// @return NULL if invalid
const char *GetClientAddr() {
return inet_ntoa(m_stClientSockaddr.sin_addr);
};
/// Returns the port number on which the client is connected.
/// @return client port number.
uint16_t GetClientPort() {
return m_stClientSockaddr.sin_port;
};
/// Returns server Internet host address as a string in standard numbers-and-dots notation.
/// @return NULL if invalid
const char *GetServerAddr() {
return inet_ntoa(m_stServerSockaddr.sin_addr);
};
/// Returns the port number on which the server is connected.
/// @return server port number.
uint16_t GetServerPort() {
return ntohs(m_stServerSockaddr.sin_port);
};
/// Get the TCP receive buffer window size for the current socket object.
/// <br><br>\b NOTE: Linux will set the receive buffer to twice the value passed.
/// @return zero on failure else the number of bytes of the TCP receive buffer window size if successful.
uint32_t GetReceiveWindowSize() {
return GetWindowSize(SO_RCVBUF);
};
/// Get the TCP send buffer window size for the current socket object.
/// <br><br>\b NOTE: Linux will set the send buffer to twice the value passed.
/// @return zero on failure else the number of bytes of the TCP receive buffer window size if successful.
uint32_t GetSendWindowSize() {
return GetWindowSize(SO_SNDBUF);
};
/// Set the TCP receive buffer window size for the current socket object.
/// <br><br>\b NOTE: Linux will set the receive buffer to twice the value passed.
/// @return zero on failure else the number of bytes of the TCP send buffer window size if successful.
uint32_t SetReceiveWindowSize(uint32_t nWindowSize) {
return SetWindowSize(SO_RCVBUF, nWindowSize);
};
/// Set the TCP send buffer window size for the current socket object.
/// <br><br>\b NOTE: Linux will set the send buffer to twice the value passed.
/// @return zero on failure else the number of bytes of the TCP send buffer window size if successful.
uint32_t SetSendWindowSize(uint32_t nWindowSize) {
return SetWindowSize(SO_SNDBUF, nWindowSize);
};
/// Disable the Nagle algorithm (Set TCP_NODELAY to true)
/// @return false if failed to set socket option otherwise return true;
bool DisableNagleAlgoritm();
/// Enable the Nagle algorithm (Set TCP_NODELAY to false)
/// @return false if failed to set socket option otherwise return true;
bool EnableNagleAlgoritm();
virtual bool Open(const char *pAddr, uint16_t nPort) {
return true;
}
///
/// \brief bindport
/// \return
///
virtual bool bindport(const char *, uint32_t);
///
/// \brief open
/// \return
///
virtual bool open();
///
/// \brief isOpen
/// \return
///
virtual bool isOpen();
///
/// \brief closePort
///
virtual void closePort();
///
/// \brief flush
///
virtual void flush();
///
/// \brief available
/// \return
///
virtual size_t available();
///
/// \brief readSize
/// \param size
/// \return
///
virtual std::string readSize(size_t size = 1);
///
/// \brief waitfordata
/// \param data_count
/// \param timeout
/// \param returned_size
/// \return
///
virtual int waitfordata(size_t data_count, uint32_t timeout = -1,
size_t *returned_size = NULL);
///
/// \brief writeData
/// \param data
/// \param size
/// \return
///
virtual size_t writeData(const uint8_t *data, size_t size);
///
/// \brief readData
/// \param data
/// \param size
/// \return
///
virtual size_t readData(uint8_t *data, size_t size);
protected:
/// Set internal socket error to that specified error
/// @param error type of error
void SetSocketError(CSimpleSocket::CSocketError error) {
m_socketErrno = error;
};
/// Set object socket handle to that specified as parameter
/// @param socket value of socket descriptor
void SetSocketHandle(SOCKET socket) {
m_socket = socket;
};
/// Flush the socket descriptor owned by the object.
/// @return true data was successfully sent, else return false;
bool Flush();
private:
/// Generic function used to get the send/receive window size
/// @return zero on failure else the number of bytes of the TCP window size if successful.
uint32_t GetWindowSize(uint32_t nOptionName);
/// Generic function used to set the send/receive window size
/// @return zero on failure else the number of bytes of the TCP window size if successful.
uint32_t SetWindowSize(uint32_t nOptionName, uint32_t nWindowSize);
/// Attempts to send at most nNumItem blocks described by sendVector
/// to the socket descriptor associated with the socket object.
/// @param sendVector pointer to an array of iovec structures
/// @param nNumItems number of items in the vector to process
/// <br>\b Note: This implementation is for systems that don't natively
/// support this functionality.
/// @return number of bytes actually sent, return of zero means the
/// connection has been shutdown on the other side, and a return of -1
/// means that an error has occurred.
int32_t Writev(const struct iovec *pVector, size_t nCount);
CSimpleSocket *operator=(CSimpleSocket &socket);
protected:
SOCKET m_socket; /// socket handle
CSocketError m_socketErrno; /// number of last error
uint8_t *m_pBuffer; /// internal send/receive buffer
int32_t
m_nBufferSize; /// size of internal send/receive buffer
int32_t m_nSocketDomain; /// socket type PF_INET, PF_INET6
CSocketType m_nSocketType; /// socket type - UDP, TCP or RAW
int32_t m_nBytesReceived; /// number of bytes received
int32_t m_nBytesSent; /// number of bytes sent
uint32_t m_nFlags; /// socket flags
bool m_bIsBlocking; /// is socket blocking
bool m_bIsMulticast; /// is the UDP socket multicast;
struct timeval m_stConnectTimeout; /// connection timeout
struct timeval m_stRecvTimeout; /// receive timeout
struct timeval m_stSendTimeout; /// send timeout
struct sockaddr_in m_stServerSockaddr; /// server address
struct sockaddr_in m_stClientSockaddr; /// client address
struct sockaddr_in m_stMulticastGroup; /// multicast group to bind to
struct linger m_stLinger; /// linger flag
CStatTimer m_timer; /// internal statistics.
#if defined(_WIN32)
WSADATA m_hWSAData; /// Windows
#endif
fd_set m_writeFds; /// write file descriptor set
fd_set m_readFds; /// read file descriptor set
fd_set m_errorFds; /// error file descriptor set
std::string m_addr;
uint32_t m_port;
bool m_open;
};
}//namespace socket
}//namespace core
}//namespace ydlidar
#endif /* __SOCKET_H__ */

150
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/StatTimer.h Normal file
View File

@@ -0,0 +1,150 @@
/*----------------------------------------------------------------------------*/
/* */
/* StatTimer.h: interface for the CStatTimer class. */
/* */
/* Author: Mark Carrier (mark@carrierlabs.com) */
/* */
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2006 CarrierLabs, LLC. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* 4. The name "CarrierLabs" must not be used to
* endorse or promote products derived from this software without
* prior written permission. For written permission, please contact
* mark@carrierlabs.com.
*
* THIS SOFTWARE IS PROVIDED BY MARK CARRIER ``AS IS'' AND ANY
* EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MARK CARRIER OR
* ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
* OF THE POSSIBILITY OF SUCH DAMAGE.
*----------------------------------------------------------------------------*/
#ifndef __CSTATTIMER_H__
#define __CSTATTIMER_H__
#include <string.h>
#if defined(_WIN32)
#include <Winsock2.h>
#include <time.h>
#else
#include <stdio.h>
#include <sys/time.h>
#endif
#if defined(_WIN32)
#if !defined (_WINSOCK2API_) && !defined(_WINSOCKAPI_)
struct timeval {
long tv_sec; /* seconds */
long tv_usec; /* and microseconds */
};
#endif
inline static int gettimeofday(struct timeval *tv, void *tz) {
union {
long long ns100;
FILETIME t;
} now;
GetSystemTimeAsFileTime(&now.t);
tv->tv_usec = long((now.ns100 / 10LL) % 1000000LL);
tv->tv_sec = long((now.ns100 - 116444736000000000LL) / 10000000LL);
return 0;
}
#undef HAS_CLOCK_GETTIME
#endif
#define GET_CLOCK_COUNT(x) gettimeofday(x, NULL)
#include <core/base/v8stdint.h>
/// Class to abstract socket communications in a cross platform manner.
/// This class is designed
class CStatTimer {
public:
CStatTimer() {
memset(&m_startTime, 0, sizeof(struct timeval));
memset(&m_endTime, 0, sizeof(struct timeval));
};
~CStatTimer() {
};
void Initialize() {
memset(&m_startTime, 0, sizeof(struct timeval));
memset(&m_endTime, 0, sizeof(struct timeval));
};
struct timeval GetStartTime() {
return m_startTime;
};
void SetStartTime() {
GET_CLOCK_COUNT(&m_startTime);
};
struct timeval GetEndTime() {
return m_endTime;
};
void SetEndTime() {
GET_CLOCK_COUNT(&m_endTime);
};
uint32_t GetMilliSeconds() {
return (CalcTotalUSec() / MILLISECONDS_CONVERSION);
};
uint64_t GetMicroSeconds() {
return (CalcTotalUSec());
};
uint32_t GetSeconds() {
return (CalcTotalUSec() / MICROSECONDS_CONVERSION);
};
static uint64_t GetCurrentTime() {
#if HAS_CLOCK_GETTIME
struct timespec tim;
clock_gettime(CLOCK_REALTIME, &tim);
return (uint64_t)(tim.tv_sec * 1000000000LL + tim.tv_nsec);
#else
struct timeval timeofday;
gettimeofday(&timeofday, NULL);
return (uint64_t)(timeofday.tv_sec * 1000000000LL + timeofday.tv_usec * 1000);
#endif
};
private:
uint32_t CalcTotalUSec() {
return (((m_endTime.tv_sec - m_startTime.tv_sec) * MICROSECONDS_CONVERSION) +
(m_endTime.tv_usec - m_startTime.tv_usec));
};
private:
struct timeval m_startTime;
struct timeval m_endTime;
};
#endif // __CSTATTIMER_H__

10
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/CMakeLists.txt Normal file
View File

@@ -0,0 +1,10 @@
aux_include_directory(. HDRS)
aux_src_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})
subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR})
foreach(subdir ${SUBDIRS})
include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/${subdir} )
add_subdirectory(${subdir})
endforeach()

48
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/common.h Normal file
View File

@@ -0,0 +1,48 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, EAIBOT, Inc.
* 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (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(_WIN32)
#include "impl\windows\win.h"
#include "impl\windows\win_serial.h"
#elif defined(__GNUC__)
#include "impl/unix/unix.h"
#include "impl/unix/unix_serial.h"
#else
#error "unsupported target"
#endif
#include <core/base/thread.h>
#include <core/base/locker.h>
#include <core/base/timer.h>

8
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/CMakeLists.txt Normal file
View File

@@ -0,0 +1,8 @@
IF (WIN32)
add_subdirectory(windows)
include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/windows)
ELSE()
add_subdirectory(unix)
include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/unix)
ENDIF()

4
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/CMakeLists.txt Normal file
View File

@@ -0,0 +1,4 @@
aux_include_directory(. HDRS)
aux_src_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})

331
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/list_ports_linux.cpp Normal file
View File

@@ -0,0 +1,331 @@
#if defined(__linux__)
/*
* Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
* This software is made available under the terms of the MIT licence.
* A copy of the licence can be obtained from:
* http://opensource.org/licenses/MIT
*/
#include <vector>
#include <string>
#include <sstream>
#include <stdexcept>
#include <iostream>
#include <fstream>
#include <cstdio>
#include <cstdarg>
#include <cstdlib>
#include <glob.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <core/serial/serial.h>
using namespace ydlidar::core;
using ydlidar::core::serial::PortInfo;
using std::istringstream;
using std::ifstream;
using std::getline;
using std::vector;
using std::string;
using std::cout;
using std::endl;
static vector<string> glob(const vector<string> &patterns);
static string basename(const string &path);
static string dirname(const string &path);
static bool path_exists(const string &path);
static string realpath(const string &path);
static string usb_sysfs_friendly_name(const string &sys_usb_path,
string &device_id);
static vector<string> get_sysfs_info(const string &device_path);
static string read_line(const string &file);
static string usb_sysfs_hw_string(const string &sysfs_path);
static string format(const char *format, ...);
vector<string>
glob(const vector<string> &patterns) {
vector<string> paths_found;
if (patterns.size() == 0) {
return paths_found;
}
glob_t glob_results;
int glob_retval = glob(patterns[0].c_str(), 0, NULL, &glob_results);
vector<string>::const_iterator iter = patterns.begin();
while (++iter != patterns.end()) {
glob_retval = glob(iter->c_str(), GLOB_APPEND, NULL, &glob_results);
}
for (size_t path_index = 0; path_index < glob_results.gl_pathc; path_index++) {
paths_found.push_back(glob_results.gl_pathv[path_index]);
}
globfree(&glob_results);
return paths_found;
}
string
basename(const string &path) {
size_t pos = path.rfind("/");
if (pos == std::string::npos) {
return path;
}
return string(path, pos + 1, string::npos);
}
string
dirname(const string &path) {
size_t pos = path.rfind("/");
if (pos == std::string::npos) {
return path;
} else if (pos == 0) {
return "/";
}
return string(path, 0, pos);
}
bool
path_exists(const string &path) {
struct stat sb;
if (stat(path.c_str(), &sb) == 0) {
return true;
}
return false;
}
string
realpath(const string &path) {
char *real_path = realpath(path.c_str(), NULL);
string result;
if (real_path != NULL) {
result = real_path;
free(real_path);
}
return result;
}
string
usb_sysfs_friendly_name(const string &sys_usb_path, string &device_id) {
unsigned int device_number = 0;
istringstream(read_line(sys_usb_path + "/devnum")) >> device_number;
string manufacturer = read_line(sys_usb_path + "/manufacturer");
string product = read_line(sys_usb_path + "/product");
string serial = read_line(sys_usb_path + "/serial");
device_id = read_line(sys_usb_path + "/devpath");
if (manufacturer.empty() && product.empty() && serial.empty()) {
return "";
}
return format("%s %s %s", manufacturer.c_str(), product.c_str(),
serial.c_str());
}
vector<string>
get_sysfs_info(const string &device_path) {
string device_name = basename(device_path);
string friendly_name;
string hardware_id;
string device_id;
string sys_device_path = format("/sys/class/tty/%s/device",
device_name.c_str());
if (device_name.compare(0, 6, "ttyUSB") == 0) {
sys_device_path = dirname(dirname(realpath(sys_device_path)));
if (path_exists(sys_device_path)) {
friendly_name = usb_sysfs_friendly_name(sys_device_path, device_id);
hardware_id = usb_sysfs_hw_string(sys_device_path);
}
} else if (device_name.compare(0, 6, "ttyACM") == 0) {
sys_device_path = dirname(realpath(sys_device_path));
if (path_exists(sys_device_path)) {
friendly_name = usb_sysfs_friendly_name(sys_device_path, device_id);
hardware_id = usb_sysfs_hw_string(sys_device_path);
}
} else {
// Try to read ID string of PCI device
string sys_id_path = sys_device_path + "/id";
if (path_exists(sys_id_path)) {
hardware_id = read_line(sys_id_path);
}
}
if (friendly_name.empty()) {
friendly_name = device_name;
}
if (hardware_id.empty()) {
hardware_id = "n/a";
}
vector<string> result;
result.push_back(friendly_name);
result.push_back(hardware_id);
result.push_back(device_id);
return result;
}
string
read_line(const string &file) {
ifstream ifs(file.c_str(), ifstream::in);
string line;
if (ifs) {
getline(ifs, line);
}
return line;
}
string
format(const char *format, ...) {
va_list ap;
size_t buffer_size_bytes = 256;
string result;
char *buffer = (char *)malloc(buffer_size_bytes);
if (buffer == NULL) {
return result;
}
bool done = false;
unsigned int loop_count = 0;
while (!done) {
va_start(ap, format);
int return_value = vsnprintf(buffer, buffer_size_bytes, format, ap);
if (return_value < 0) {
done = true;
} else if (return_value >= (int)buffer_size_bytes) {
// Realloc and try again.
buffer_size_bytes = return_value + 1;
char *new_buffer_ptr = (char *)realloc(buffer, buffer_size_bytes);
if (new_buffer_ptr == NULL) {
done = true;
} else {
buffer = new_buffer_ptr;
}
} else {
result = buffer;
done = true;
}
va_end(ap);
if (++loop_count > 5) {
done = true;
}
}
free(buffer);
return result;
}
string
usb_sysfs_hw_string(const string &sysfs_path) {
string serial_number = read_line(sysfs_path + "/serial");
if (serial_number.length() > 0) {
serial_number = format("SNR=%s", serial_number.c_str());
}
string vid = read_line(sysfs_path + "/idVendor");
string pid = read_line(sysfs_path + "/idProduct");
return format("USB VID:PID=%s:%s %s", vid.c_str(), pid.c_str(),
serial_number.c_str());
}
vector<PortInfo>
serial::list_ports()
{
vector<PortInfo> results;
vector<string> search_globs;
search_globs.push_back("/dev/ttyACM*");
search_globs.push_back("/dev/ttyS*");
search_globs.push_back("/dev/ttyUSB*");
search_globs.push_back("/dev/tty.*");
search_globs.push_back("/dev/cu.*");
vector<string> devices_found = glob(search_globs);
vector<string>::iterator iter = devices_found.begin();
while (iter != devices_found.end()) {
string device = *iter++;
vector<string> sysfs_info = get_sysfs_info(device);
string friendly_name = sysfs_info[0];
string hardware_id = sysfs_info[1];
string device_id = sysfs_info[2];
std::size_t found = hardware_id.find("10c4:ea60");
std::size_t found1 = hardware_id.find("0483:5740");
// if (found != std::string::npos || found1 != std::string::npos)
{
PortInfo device_entry;
device_entry.port = device;
device_entry.description = friendly_name;
device_entry.hardware_id = hardware_id;
device_entry.device_id = device_id;
results.push_back(device_entry);
}
}
return results;
}
#endif // defined(__linux__)

796
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.c Normal file
View File

@@ -0,0 +1,796 @@
/*
* lock.c
*
* Created on: 28 Mar 2018
* Author: Tony
* E-Mail: Tony@gmail.com
*/
#include "lock.h"
#ifndef WIN32
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/sysmacros.h>
#include <fcntl.h>
#include <string.h>
#include <limits.h>
#include <stdlib.h>
#include <signal.h>
#ifdef LFS
/*----------------------------------------------------------
lfs_lock
accept: The name of the device to try to lock
perform: Create a lock file if there is not one already using a
lock file server.
return: 1 on failure 0 on success
exceptions: none
comments:
----------------------------------------------------------*/
int lfs_lock(const char *filename, int pid) {
int s;
int ret;
int size = 1024;
char *buffer = malloc(size);
struct sockaddr_in addr;
if (!(s = socket(AF_INET, SOCK_STREAM, 0)) > 0) {
free(buffer);
return 1;
}
addr.sin_family = AF_INET;
addr.sin_port = htons(50001);
addr.sin_addr.s_addr = inet_addr("127.0.0.1");
if (!connect(s, (struct sockaddr *) &addr, sizeof(addr)) == 0) {
free(buffer);
return 1;
}
ret = recv(s, buffer, size, 0);
sprintf(buffer, "lock %s %i\n", filename, pid);
/* printf( "%s", buffer ); */
send(s, buffer, strlen(buffer), 0);
ret = recv(s, buffer, size, 0);
if (ret > 0) {
buffer[ret] = '\0';
/* printf( "Message recieved: %s", buffer ); */
}
send(s, "quit\n", strlen("quit\n"), 0);
close(s);
/* printf("%s\n", buffer); */
if (buffer[0] == '2') {
return 0;
}
free(buffer);
return 1;
}
/*----------------------------------------------------------
lfs_unlock
accept: The name of the device to try to unlock
perform: Remove a lock file if there is one using a
lock file server.
return: 1 on failure 0 on success
exceptions: none
comments:
----------------------------------------------------------*/
int lfs_unlock(const char *filename, int pid) {
int s;
int ret;
int size = 1024;
char *buffer = malloc(size);
struct sockaddr_in addr;
if (!(s = socket(AF_INET, SOCK_STREAM, 0)) > 0) {
free(buffer);
return 1;
}
addr.sin_family = AF_INET;
addr.sin_port = htons(50001);
addr.sin_addr.s_addr = inet_addr("127.0.0.1");
if (!connect(s, (struct sockaddr *) &addr, sizeof(addr)) == 0) {
free(buffer);
return 1;
}
sprintf(buffer, "unlock %s %i\n", filename, pid);
/* printf( "%s", buffer ); */
send(s, buffer, strlen(buffer), 0);
ret = recv(s, buffer, size, 0);
if (ret > 0) {
buffer[ret] = '\0';
/* printf( "Message recieved: %s", buffer ); */
}
send(s, "quit\n", strlen("quit\n"), 0);
close(s);
if (buffer[0] == '2') {
return 0;
}
free(buffer);
return 1;
}
#endif /* LFS */
/*----------------------------------------------------------
lib_lock_dev_unlock
accept: The name of the device to try to unlock
perform: Remove a lock file if there is one using a
lock file server.
return: 1 on failure 0 on success
exceptions: none
comments: This is for use with liblockdev which comes with Linux
distros. I suspect it will be problematic with embeded
Linux. taj
----------------------------------------------------------*/
#ifdef LIBLOCKDEV
int lib_lock_dev_unlock(const char *filename, int pid) {
if (dev_unlock(filename, pid)) {
//report("fhs_unlock: Unable to remove LockFile\n");
return (1);
}
return (0);
}
#endif /* LIBLOCKDEV */
/*----------------------------------------------------------
lib_lock_dev_lock
accept: The name of the device to try to lock
termios struct
perform: Create a lock file if there is not one already.
return: 1 on failure 0 on success
exceptions: none
comments: This is for use with liblockdev which comes with Linux
distros. I suspect it will be problematic with embeded
Linux. taj
One could load the library here rather than link it and
always try to use this.
----------------------------------------------------------*/
#ifdef LIBLOCKDEV
int lib_lock_dev_lock(const char *filename, int pid) {
char message[80];
printf("LOCKING %s\n", filename);
if (dev_testlock(filename)) {
//report( "fhs_lock() lockstatus fail\n" );
return 1;
}
if (dev_lock(filename)) {
sprintf(message,
"RXTX fhs_lock() Error: creating lock file for: %s: %s\n",
filename, strerror(errno));
// report_error( message );
return 1;
}
return (0);
}
#endif /* LIBLOCKDEV */
/*----------------------------------------------------------
fhs_lock
accept: The name of the device to try to lock
termios struct
perform: Create a lock file if there is not one already.
return: 1 on failure 0 on success
exceptions: none
comments: This is for linux and freebsd only currently. I see SVR4 does
this differently and there are other proposed changes to the
Filesystem Hierachy Standard
more reading:
----------------------------------------------------------*/
int fhs_lock(const char *filename, int pid) {
/*
* There is a zoo of lockdir possibilities
* Its possible to check for stale processes with most of them.
* for now we will just check for the lockfile on most
* Problem lockfiles will be dealt with. Some may not even be in use.
*
*/
int fd, j;
char lockinfo[12];
char file[80], *p;
j = strlen(filename);
p = (char *) filename + j;
/* FIXME need to handle subdirectories /dev/cua/...
SCO Unix use lowercase all the time
taj
*/
while (*(p - 1) != '/' && j-- != 1) {
#if defined ( __unixware__ )
*p = tolower(*p);
#endif /* __unixware__ */
p--;
}
sprintf(file, "%s/LCK..%s", LOCKDIR, p);
if (check_lock_status(filename)) {
printf("fhs_lock() lockstatus fail\n");
return 1;
}
fd = open(file, O_CREAT | O_WRONLY | O_EXCL, 0444);
if (fd < 0) {
printf(
"RXTX fhs_lock() Error: creating lock file: %s: %s\n",
file, strerror(errno));
return 1;
}
sprintf(lockinfo, "%10d\n", (int) getpid());
printf("fhs_lock: creating lockfile: %s\n", lockinfo);
write(fd, lockinfo, 11);
close(fd);
return 0;
}
/*----------------------------------------------------------
uucp_lock
accept: char * filename. Device to be locked
perform: Try to get a uucp_lock
return: int 0 on success
exceptions: none
comments:
The File System Hierarchy Standard
http://www.pathname.com/fhs/
UUCP Lock Files
http://docs.freebsd.org/info/uucp/uucp.info.UUCP_Lock_Files.html
FSSTND
ftp://tsx-11.mit.edu/pub/linux/docs/linux-standards/fsstnd/
Proposed Changes to the File System Hierarchy Standard
ftp://scicom.alphacdc.com/pub/linux/devlock-0.X.tgz
"UNIX Network Programming", W. Richard Stevens,
Prentice-Hall, 1990, pages 96-101.
There is much to do here.
1) UUCP style locks (done)
/var/spool/uucp
2) SVR4 locks
/var/spool/locks
3) FSSTND locks (done)
/var/lock
4) handle stale locks (done except kermit locks)
5) handle minicom lockfile contents (FSSTND?)
" 16929 minicom root\n" (done)
6) there are other Lock conventions that use Major and Minor
numbers...
7) Stevens recommends LCK..<pid>
most are caught above. If they turn out to be problematic
rather than an exercise, we will handle them.
----------------------------------------------------------*/
int uucp_lock(const char *filename, int pid) {
char lockfilename[80], lockinfo[12];
char name[80];
int fd;
struct stat buf;
printf("uucp_lock( %s );\n", filename);
if (check_lock_status(filename)) {
printf("RXTX uucp check_lock_status true\n");
return 1;
}
if (stat(LOCKDIR, &buf) != 0) {
printf("RXTX uucp_lock() could not find lock directory.\n");
return 1;
}
if (stat(filename, &buf) != 0) {
printf("RXTX uucp_lock() could not find device.\n");
printf("uucp_lock: device was %s\n", name);
return 1;
}
sprintf(lockfilename, "%s/LK.%03d.%03d.%03d",
LOCKDIR,
(int) major(buf.st_dev),
(int) major(buf.st_rdev),
(int) minor(buf.st_rdev)
);
sprintf(lockinfo, "%10d\n", (int) getpid());
if (stat(lockfilename, &buf) == 0) {
printf("RXTX uucp_lock() %s is there\n",
lockfilename);
return 1;
}
fd = open(lockfilename, O_CREAT | O_WRONLY | O_EXCL, 0444);
if (fd < 0) {
printf(
"RXTX uucp_lock() Error: creating lock file: %s\n",
lockfilename);
return 1;
}
write(fd, lockinfo, 11);
close(fd);
return 0;
}
/*----------------------------------------------------------
check_lock_status
accept: the lock name in question
perform: Make sure everything is sane
return: 0 on success
exceptions: none
comments:
----------------------------------------------------------*/
int check_lock_status(const char *filename) {
struct stat buf;
/* First, can we find the directory? */
if (stat(LOCKDIR, &buf) != 0) {
printf("check_lock_status: could not find lock directory.\n");
return 1;
}
/* OK. Are we able to write to it? If not lets bail */
if (check_group_uucp()) {
printf("check_lock_status: No permission to create lock file.\nplease see: How can I use Lock Files with rxtx? in INSTALL\n");
return (1);
}
/* is the device alread locked */
if (is_device_locked(filename)) {
printf("check_lock_status: device is locked by another application\n");
return 1;
}
return 0;
}
/*----------------------------------------------------------
fhs_unlock
accept: The name of the device to unlock
perform: delete the lock file
return: none
exceptions: none
comments: This is for linux only currently. I see SVR4 does this
differently and there are other proposed changes to the
Filesystem Hierachy Standard
----------------------------------------------------------*/
void fhs_unlock(const char *filename, int openpid) {
char file[80], *p;
int i;
i = strlen(filename);
p = (char *) filename + i;
/* FIXME need to handle subdirectories /dev/cua/... */
while (*(p - 1) != '/' && i-- != 1) {
p--;
}
sprintf(file, "%s/LCK..%s", LOCKDIR, p);
if (!check_lock_pid(file, openpid)) {
unlink(file);
printf("fhs_unlock: Removing LockFile\n");
} else {
printf("fhs_unlock: Unable to remove LockFile\n");
}
}
/*----------------------------------------------------------
uucp_unlock
accept: char *filename the device that is locked
perform: remove the uucp lockfile if it exists
return: none
exceptions: none
comments: http://docs.freebsd.org/info/uucp/uucp.info.UUCP_Lock_Files.html
----------------------------------------------------------*/
void uucp_unlock(const char *filename, int openpid) {
struct stat buf;
char file[80];
/* FIXME */
printf("uucp_unlock( %s );\n", filename);
if (stat(filename, &buf) != 0) {
/* hmm the file is not there? */
printf("uucp_unlock() no such device\n");
return;
}
sprintf(file, LOCKDIR"/LK.%03d.%03d.%03d",
(int) major(buf.st_dev),
(int) major(buf.st_rdev),
(int) minor(buf.st_rdev)
);
if (stat(file, &buf) != 0) {
/* hmm the file is not there? */
printf("uucp_unlock no such lockfile\n");
return;
}
if (!check_lock_pid(file, openpid)) {
printf("uucp_unlock: unlinking %s\n", file);
unlink(file);
} else {
printf("uucp_unlock: unlinking failed %s\n", file);
}
}
/*----------------------------------------------------------
check_lock_pid
accept: the name of the lockfile
perform: make sure the lock file is ours.
return: 0 on success
exceptions: none
comments:
----------------------------------------------------------*/
int check_lock_pid(const char *file, int openpid) {
int fd, lockpid;
char pid_buffer[12];
fd = open(file, O_RDONLY);
if (fd < 0) {
return (1);
}
if (read(fd, pid_buffer, 11) < 0) {
close(fd);
return (1);
}
close(fd);
pid_buffer[11] = '\0';
lockpid = atol(pid_buffer);
/* Native threads JVM's have multiple pids */
if (lockpid != getpid() && lockpid != getppid() && lockpid != openpid) {
printf("check_lock_pid: lock = %s pid = %i gpid=%i openpid=%i\n",
pid_buffer, (int) getpid(), (int) getppid(), openpid);
return (1);
}
return (0);
}
/*----------------------------------------------------------
check_group_uucp
accept: none
perform: check if the user is root or in group uucp
return: 0 on success
exceptions: none
comments:
This checks if the effective user is in group uucp so we can
create lock files. If not we give them a warning and bail.
If its root we just skip the test.
if someone really wants to override this they can use the USER_LOCK_DIRECTORY --not recommended.
In a recent change RedHat 7.2 decided to use group lock.
In order to get around this we just check the group id
of the lock directory.
* Modified to support Debian *
The problem was that checking the ownership of the lock file
dir is not enough, in the sense that even if the current user
is not in the group of the lock directory if the lock
directory has 777 permissions the lock file can be anyway
created. My solution is simply to try to create a tmp file
there and if it works then we can go on. Here is my code that
I tried and seems to work.
Villa Valerio <valerio.villa@siemens.com>
----------------------------------------------------------*/
int check_group_uucp() {
#ifndef USER_LOCK_DIRECTORY
FILE *testLockFile ;
char testLockFileDirName[] = LOCKDIR;
char testLockFileName[] = "tmpXXXXXX";
char *testLockAbsFileName;
testLockAbsFileName = calloc(strlen(testLockFileDirName)
+ strlen(testLockFileName) + 2, sizeof(char));
if (NULL == testLockAbsFileName) {
printf("check_group_uucp(): Insufficient memory");
return 1;
}
strcat(testLockAbsFileName, testLockFileDirName);
strcat(testLockAbsFileName, "/");
strcat(testLockAbsFileName, testLockFileName);
if (-1 == mkstemp(testLockAbsFileName)) {
free(testLockAbsFileName);
printf("check_group_uucp(): mktemp malformed string - \
should not happen");
return 1;
}
testLockFile = fopen(testLockAbsFileName, "w+");
if (NULL == testLockFile) {
printf("check_group_uucp(): error testing lock file "
"creation Error details:");
printf("%s\n", strerror(errno));
free(testLockAbsFileName);
return 1;
}
fclose(testLockFile);
unlink(testLockAbsFileName);
free(testLockAbsFileName);
#endif /* USER_LOCK_DIRECTORY */
return 0;
}
#ifdef USE_OLD_CHECK_GROUP_UUCP
int check_group_uucp() {
#ifndef USER_LOCK_DIRECTORY
int group_count;
struct passwd *user = getpwuid(geteuid());
struct stat buf;
char msg[80];
gid_t list[ NGROUPS_MAX ];
if (stat(LOCKDIR, &buf)) {
sprintf(msg, "check_group_uucp: Can not find Lock Directory: %s\n", LOCKDIR);
printf(msg);
return (1);
}
group_count = getgroups(NGROUPS_MAX, list);
list[ group_count ] = geteuid();
/* JJO changes start */
if (user == NULL) {
printf("Not able to get user groups.\n");
return 1;
} else
/* JJO changes stop */
if (user->pw_gid) {
while (group_count >= 0 && buf.st_gid != list[ group_count ]) {
group_count--;
}
if (buf.st_gid == list[ group_count ]) {
return 0;
}
sprintf(msg, "%i %i\n", buf.st_gid, list[ group_count ]);
printf(msg);
printf(UUCP_ERROR);
return 1;
}
return 0;
/*
if( strcmp( user->pw_name, "root" ) )
{
while( *g->gr_mem )
{
if( !strcmp( *g->gr_mem, user->pw_name ) )
{
break;
}
(void) *g->gr_mem++;
}
if( !*g->gr_mem )
{
printf( UUCP_ERROR );
return 1;
}
}
*/
#endif /* USER_LOCK_DIRECTORY */
return 0;
}
#endif /* USE_OLD_CHECK_GROUP_UUCP */
/*----------------------------------------------------------
The following should be able to follow symbolic links. I think the stat
method used below will work on more systems. This was found while looking
for information.
* realpath() doesn't exist on all of the systems my code has to run
on (HP-UX 9.x, specifically)
----------------------------------------------------------
int different_from_LOCKDIR(const char* ld)
{
char real_ld[MAXPATHLEN];
char real_LOCKDIR[MAXPATHLEN];
if (strncmp(ld, LOCKDIR, strlen(ld)) == 0)
return 0;
if (realpath(ld, real_ld) == NULL)
return 1;
if (realpath(LOCKDIR, real_LOCKDIR) == NULL)
return 1;
if (strncmp(real_ld, real_LOCKDIR, strlen(real_ld)) == 0)
return 0;
else
return 1;
}
*/
/*----------------------------------------------------------
is_device_locked
accept: char * filename. The device in question including the path.
perform: see if one of the many possible lock files is aready there
if there is a stale lock, remove it.
return: 1 if the device is locked or somethings wrong.
0 if its possible to create our own lock file.
exceptions: none
comments: check if the device is already locked
----------------------------------------------------------*/
int is_device_locked(const char *port_filename) {
const char *lockdirs[] = { "/etc/locks", "/usr/spool/kermit",
"/usr/spool/locks", "/usr/spool/uucp", "/usr/spool/uucp/",
"/usr/spool/uucp/LCK", "/var/lock", "/var/lock/modem",
"/var/spool/lock", "/var/spool/locks", "/var/spool/uucp",
LOCKDIR, NULL
};
const char *lockprefixes[] = { "LCK..", "lk..", "LK.", NULL };
char *p, file[80], pid_buffer[20];
int i = 0, j, k, fd, pid;
struct stat buf, buf2, lockbuf;
j = strlen(port_filename);
p = (char *) port_filename + j;
while (*(p - 1) != '/' && j-- != 1) {
p--;
}
stat(LOCKDIR, &lockbuf);
while (lockdirs[i]) {
/*
Look for lockfiles in all known places other than the
defined lock directory for this system
report any unexpected lockfiles.
Is the suspect lockdir there?
if it is there is it not the expected lock dir?
*/
if (!stat(lockdirs[i], &buf2) &&
buf2.st_ino != lockbuf.st_ino &&
strncmp(lockdirs[i], LOCKDIR, strlen(lockdirs[i]))) {
j = strlen(port_filename);
p = (char *) port_filename + j;
/*
SCO Unix use lowercase all the time
taj
*/
while (*(p - 1) != '/' && j-- != 1) {
#if defined ( __unixware__ )
*p = tolower(*p);
#endif /* __unixware__ */
p--;
}
k = 0;
while (lockprefixes[k]) {
/* FHS style */
sprintf(file, "%s/%s%s", lockdirs[i],
lockprefixes[k], p);
if (stat(file, &buf) == 0) {
// sprintf( message, UNEXPECTED_LOCK_FILE,
// file );
// printf( message );
return 1;
}
/* UUCP style */
stat(port_filename, &buf);
sprintf(file, "%s/%s%03d.%03d.%03d",
lockdirs[i],
lockprefixes[k],
(int) major(buf.st_dev),
(int) major(buf.st_rdev),
(int) minor(buf.st_rdev)
);
if (stat(file, &buf) == 0) {
// sprintf( message, UNEXPECTED_LOCK_FILE,
// file );
// printf( message );
return 1;
}
k++;
}
}
i++;
}
/*
OK. We think there are no unexpect lock files for this device
Lets see if there any stale lock files that need to be
removed.
*/
#ifdef FHS
/* FHS standard locks */
i = strlen(port_filename);
p = (char *) port_filename + i;
while (*(p - 1) != '/' && i-- != 1) {
#if defined ( __unixware__ )
*p = tolower(*p);
#endif /* __unixware__ */
p--;
}
sprintf(file, "%s/%s%s", LOCKDIR, LOCKFILEPREFIX, p);
#else
/* UUCP standard locks */
if (stat(port_filename, &buf) != 0) {
printf("RXTX is_device_locked() could not find device.\n");
return 1;
}
sprintf(file, "%s/LK.%03d.%03d.%03d",
LOCKDIR,
(int) major(buf.st_dev),
(int) major(buf.st_rdev),
(int) minor(buf.st_rdev)
);
#endif /* FHS */
if (stat(file, &buf) == 0) {
/* check if its a stale lock */
fd = open(file, O_RDONLY);
read(fd, pid_buffer, 11);
/* FIXME null terminiate pid_buffer? need to check in Solaris */
close(fd);
sscanf(pid_buffer, "%d", &pid);
if (kill((pid_t) pid, 0) && errno == ESRCH) {
printf(
"RXTX Warning: Removing stale lock file. %s\n",
file);
if (unlink(file) != 0) {
printf("RXTX Error: Unable to \
remove stale lock file: %s\n",
file
);
return 1;
}
}
}
return 0;
}
#endif /* WIN32 */

241
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.h Normal file
View File

@@ -0,0 +1,241 @@
/*
* lock.h
*
* Created on: 28 Mar 2018
* Author: Tony
* E-Mail: Tony@gmail.com
*/
#ifndef LOCK_H_
#define LOCK_H_
#ifdef HAVE_SYS_FILE_H
# include <sys/file.h>
#endif /* HAVE_SYS_FILE_H */
#ifdef LFS /* File Lock Server */
# include <sys/socket.h>
# include <netinet/in.h>
# include <arpa/inet.h>
#endif /* FLS */
#if defined(__linux__)
# include <linux/types.h> /* fix for linux-2.3.4? kernels */
# include <linux/serial.h>
# include <linux/version.h>
#endif /* __linux__ */
#if defined(__sun__)
# include <sys/filio.h>
# include <sys/mkdev.h>
#endif /* __sun__ */
#if defined(__hpux__)
# include <sys/modem.h>
#endif /* __hpux__ */
/* FIXME -- new file */
#if defined(__APPLE__)
# include <CoreFoundation/CoreFoundation.h>
# include <IOKit/IOKitLib.h>
# include <IOKit/serial/IOSerialKeys.h>
# include <IOKit/IOBSD.h>
#endif /* __APPLE__ */
#ifdef __unixware__
# include <sys/filio.h>
#endif /* __unixware__ */
#ifdef HAVE_PWD_H
#include <pwd.h>
#endif /* HAVE_PWD_H */
#ifdef HAVE_GRP_H
#include <grp.h>
#endif /* HAVE_GRP_H */
#include <math.h>
#ifdef LIBLOCKDEV
#include <lockdev.h>
#endif /* LIBLOCKDEV */
/* Ports known on the OS */
#if defined(__linux__)
# define DEVICEDIR "/dev/"
# define LOCKDIR "/var/lock"
# define LOCKFILEPREFIX "LCK.."
# define FHS
#endif /* __linux__ */
#if defined(__QNX__)
# define DEVICEDIR "/dev/"
# define LOCKDIR ""
# define LOCKFILEPREFIX ""
#endif /* qnx */
#if defined(__sgi__) || defined(sgi)
# define DEVICEDIR "/dev/"
# define LOCKDIR "/usr/spool/uucp"
# define LOCKFILEPREFIX "LK."
# define UUCP
#endif /* __sgi__ || sgi */
#if defined(__FreeBSD__)
# define DEVICEDIR "/dev/"
# define LOCKDIR "/var/spool/lock"
# define LOCKFILEPREFIX "LK.."
# define UUCP
#endif /* __FreeBSD__ */
#if defined(__APPLE__)
# define DEVICEDIR "/dev/"
/*# define LOCKDIR "/var/spool/uucp"*/
# define LOCKDIR "/var/lock"
# define LOCKFILEPREFIX "LK."
# define UUCP
#endif /* __APPLE__ */
#if defined(__NetBSD__)
# define DEVICEDIR "/dev/"
# define LOCKDIR "/var/lock"
/*# define LOCKDIR "/usr/spool/uucp"*/
# define LOCKFILEPREFIX "LK."
# define UUCP
#endif /* __NetBSD__ */
#if defined(__unixware__)
# define DEVICEDIR "/dev/"
/* really this only fully works for OpenServer */
# define LOCKDIR "/var/spool/uucp/"
# define LOCKFILEPREFIX "LK."
/*
this needs work....
Jonathan Schilling <jls@caldera.com> writes:
This is complicated because as I said in my previous mail, there are
two kinds of SCO operating systems.
The one that most people want gnu.io for, including the guy who
asked the mailing list about SCO support a few days ago, is Open Server
(a/k/a "SCO UNIX"), which is SVR3-based. This uses old-style uucp locks,
of the form LCK..tty0a. That's what I implemented in the RXTX port I did,
and it works correctly.
The other SCO/Caldera OS, UnixWare/Open UNIX, uses the new-style
SVR4 locks, of the form LK.123.123.123. These OSes are a lot like
Solaris (UnixWare/Open UNIX come from AT&T SVR4 which had a joint
The other SCO/Caldera OS, UnixWare/Open UNIX, uses the new-style
SVR4 locks, of the form LK.123.123.123. These OSes are a lot like
Solaris (UnixWare/Open UNIX come from AT&T SVR4 which had a joint
heritage with Sun way back when). I saw that you added support
for this form of lock by RXTX 1.4-10 ... but it gets messy because,
as I said before, we use the same binary gnu.io files for both
UnixWare/Open UNIX and OpenServer. Thus we can't #ifdef one or the
other; it would have to be a runtime test. Your code and your macros
aren't set up for doing this (understandably!). So I didn't implement
these; the gnu.io locks won't fully work on UnixWare/Open UNIX
as a result, which I mentioned in the Release Notes.
What I would suggest is that you merge in the old-style LCK..tty0a lock
code that I used, since this will satisfy 90% of the SCO users. Then
I'll work on some way of getting UnixWare/Open UNIX locking to work
correctly, and give you those changes at a later date.
Jonathan
FIXME The lock type could be passed with -DOLDUUCP or -DUUCP based on
os.name in configure.in or perhaps system defines could determine the lock
type.
Trent
*/
# define OLDUUCP
#endif
#if defined(__hpux__)
/* modif cath */
# define DEVICEDIR "/dev/"
# define LOCKDIR "/var/spool/uucp"
# define LOCKFILEPREFIX "LCK."
# define UUCP
#endif /* __hpux__ */
#if defined(__osf__) /* Digital Unix */
# define DEVICEDIR "/dev/"
# define LOCKDIR ""
# define LOCKFILEPREFIX "LK."
# define UUCP
#endif /* __osf__ */
#if defined(__sun__) /* Solaris */
# define DEVICEDIR "/dev/"
# define LOCKDIR "/var/spool/locks"
# define LOCKFILEPREFIX "LK."
/*
# define UUCP
*/
#endif /* __sun__ */
#if defined(__BEOS__)
# define DEVICEDIR "/dev/ports/"
# define LOCKDIR ""
# define LOCKFILEPREFIX ""
# define UUCP
#endif /* __BEOS__ */
#if defined(WIN32)
# define DEVICEDIR "//./"
# define LOCKDIR ""
# define LOCKFILEPREFIX ""
#endif /* WIN32 */
/* allow people to override the directories */
/* #define USER_LOCK_DIRECTORY "/home/tjarvi/1.5/build" */
#ifdef USER_LOCK_DIRECTORY
# define LOCKDIR USER_LOCK_DIRECTORY
#endif /* USER_LOCK_DIRECTORY */
#ifdef DISABLE_LOCKFILES
#undef UUCP
#undef FHS
#undef OLDUUCP
#endif /* DISABLE_LOCKFILES */
/* That should be all you need to look at in this file for porting */
#ifdef LFS /* Use a Lock File Server */
# define LOCK lfs_lock
# define UNLOCK lfs_unlock
#elif defined(UUCP)
# define LOCK uucp_lock
# define UNLOCK uucp_unlock
#elif defined(OLDUUCP)
/*
We can handle the old style if needed here see __unixware__ above.
defaulting to rxtx-1.4-8 behavior for now.
see also __sco__ in SerialImp.c when changing this for a possible
bug
FIXME
*/
# define LOCK fhs_lock
# define UNLOCK fhs_unlock
#elif defined(FHS)
#ifdef LIBLOCKDEV
# define LOCK lib_lock_dev_lock
# define UNLOCK lib_lock_dev_unlock
#else
# define LOCK fhs_lock
# define UNLOCK fhs_unlock
#endif /* LIBLOCKDEV */
#else
# define LOCK system_does_not_lock
# define UNLOCK system_does_not_unlock
#endif /* UUCP */
//#ifndef __WIN32__
//#define UUCP_LOCK_DIR "/var/lock"
#ifdef __cplusplus
extern "C" {
#endif
//int uucp_lock( const char *file);
//int uucp_unlock(void);
int check_group_uucp();
int check_lock_pid(const char *file, int openpid);
int lock_device(const char *);
void unlock_device(const char *);
int is_device_locked(const char *);
int check_lock_status(const char *);
int lfs_unlock(const char *, int);
int lfs_lock(const char *, int);
int lib_lock_dev_unlock(const char *, int);
int lib_lock_dev_lock(const char *, int);
void fhs_unlock(const char *, int);
int fhs_lock(const char *, int);
void uucp_unlock(const char *, int);
int uucp_lock(const char *, int);
#ifdef __cplusplus
}
#endif
//#endif
#endif /* LOCK_H_ */

27
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix.h Normal file
View File

@@ -0,0 +1,27 @@
#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>

1634
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

162
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.h Normal file
View File

@@ -0,0 +1,162 @@
#if !defined(_WIN32)
#ifndef SERIAL_IMPL_UNIX_H
#define SERIAL_IMPL_UNIX_H
#include <pthread.h>
#include <assert.h>
#include <termios.h>
#include <core/serial/serial.h>
namespace ydlidar {
namespace core {
namespace serial {
using std::size_t;
using std::string;
class MillisecondTimer {
public:
explicit MillisecondTimer(const uint32_t millis);
int64_t remaining();
private:
static timespec timespec_now();
timespec expiry;
};
class Serial::SerialImpl {
public:
explicit SerialImpl(const string &port,
unsigned long baudrate,
bytesize_t bytesize,
parity_t parity,
stopbits_t stopbits,
flowcontrol_t flowcontrol);
virtual ~SerialImpl();
bool open();
Serial::SerialPortError getSystemError(int systemErrorCode) const;
void close();
bool isOpen() const;
size_t available();
bool waitReadable(uint32_t timeout);
void waitByteTimes(size_t count);
int waitfordata(size_t data_count, uint32_t timeout, size_t *returned_size);
size_t read(uint8_t *buf, size_t size = 1);
size_t write(const uint8_t *data, size_t length);
void flush();
void flushInput();
void flushOutput();
void sendBreak(int duration);
bool setBreak(bool level);
bool setRTS(bool level);
bool setDTR(bool level);
bool waitForChange();
bool getCTS();
bool getDSR();
bool getRI();
bool getCD();
uint32_t getByteTime();
void setPort(const string &port);
string getPort() const;
void setTimeout(Timeout &timeout);
Timeout getTimeout() const;
bool setBaudrate(unsigned long baudrate);
bool setStandardBaudRate(speed_t baudrate);
bool setCustomBaudRate(unsigned long baudrate);
unsigned long getBaudrate() const;
bool setBytesize(bytesize_t bytesize);
bytesize_t getBytesize() const;
bool setParity(parity_t parity);
parity_t getParity() const;
bool setStopbits(stopbits_t stopbits);
stopbits_t getStopbits() const;
bool setFlowcontrol(flowcontrol_t flowcontrol);
flowcontrol_t getFlowcontrol() const;
bool setTermios(const termios *tio);
bool getTermios(termios *tio);
int readLock();
int readUnlock();
int writeLock();
int writeUnlock();
private:
string port_; // Path to the file descriptor
int fd_; // The current file descriptor
pid_t pid;
bool is_open_;
bool xonxoff_;
bool rtscts_;
Timeout timeout_; // Timeout for read operations
unsigned long baudrate_; // Baudrate
uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte
parity_t parity_; // Parity
bytesize_t bytesize_; // Size of the bytes
stopbits_t stopbits_; // Stop Bits
flowcontrol_t flowcontrol_; // Flow Control
// Mutex used to lock the read functions
pthread_mutex_t read_mutex;
// Mutex used to lock the write functions
pthread_mutex_t write_mutex;
};
}
}
}
#endif // SERIAL_IMPL_UNIX_H
#endif // !defined(_WIN32)

5
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/CMakeLists.txt Normal file
View File

@@ -0,0 +1,5 @@
aux_include_directory(. HDRS)
aux_src_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})

196
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/list_ports_win.cpp Normal file
View File

@@ -0,0 +1,196 @@
#if defined(_WIN32)
#pragma comment(lib, "setupapi.lib")
#ifdef QT_VERSION
#include <QtGlobal>
#if (QT_VERSION <= QT_VERSION_CHECK(5,9,0))
#undef UNICODE
#endif
#else
#undef UNICODE
#endif
#include <core/serial/serial.h>
#include <tchar.h>
#include <windows.h>
#include <setupapi.h>
#include <initguid.h>
#include <devguid.h>
#include <cstring>
using ydlidar::core::serial::PortInfo;
using std::vector;
using std::string;
using namespace ydlidar::core;
static const DWORD port_name_max_length = 256;
static const DWORD friendly_name_max_length = 256;
static const DWORD hardware_id_max_length = 256;
static const DWORD device_id_max_length = 256;
// Convert a wide Unicode string to an UTF8 string
std::string utf8_encode(const std::wstring &wstr) {
int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(),
NULL, 0, NULL, NULL);
std::string strTo(size_needed, 0);
WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0],
size_needed, NULL, NULL);
return strTo;
}
vector<PortInfo>
serial::list_ports() {
vector<PortInfo> devices_found;
HDEVINFO device_info_set = SetupDiGetClassDevs(
(const GUID *) &GUID_DEVCLASS_PORTS,
NULL,
NULL,
DIGCF_PRESENT);
unsigned int device_info_set_index = 0;
SP_DEVINFO_DATA device_info_data;
device_info_data.cbSize = sizeof(SP_DEVINFO_DATA);
while (SetupDiEnumDeviceInfo(device_info_set, device_info_set_index,
&device_info_data)) {
device_info_set_index++;
// Get port name
HKEY hkey = SetupDiOpenDevRegKey(
device_info_set,
&device_info_data,
DICS_FLAG_GLOBAL,
0,
DIREG_DEV,
KEY_READ);
TCHAR port_name[port_name_max_length];
DWORD port_name_length = port_name_max_length;
LONG return_code = RegQueryValueEx(
hkey,
_T("PortName"),
NULL,
NULL,
(LPBYTE)port_name,
&port_name_length);
RegCloseKey(hkey);
if (return_code != EXIT_SUCCESS) {
continue;
}
if (port_name_length > 0 && port_name_length <= port_name_max_length) {
port_name[port_name_length - 1] = '\0';
} else {
port_name[0] = '\0';
}
// Ignore parallel ports
if (_tcsstr(port_name, _T("LPT")) != NULL) {
continue;
}
// Get port friendly name
TCHAR friendly_name[friendly_name_max_length];
DWORD friendly_name_actual_length = 0;
BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty(
device_info_set,
&device_info_data,
SPDRP_FRIENDLYNAME,
NULL,
(PBYTE)friendly_name,
friendly_name_max_length,
&friendly_name_actual_length);
if (got_friendly_name == TRUE && friendly_name_actual_length > 0) {
friendly_name[friendly_name_actual_length - 1] = '\0';
} else {
friendly_name[0] = '\0';
}
// Get hardware ID
TCHAR hardware_id[hardware_id_max_length];
DWORD hardware_id_actual_length = 0;
BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty(
device_info_set,
&device_info_data,
SPDRP_HARDWAREID,
NULL,
(PBYTE)hardware_id,
hardware_id_max_length,
&hardware_id_actual_length);
if (got_hardware_id == TRUE && hardware_id_actual_length > 0) {
hardware_id[hardware_id_actual_length - 1] = '\0';
} else {
hardware_id[0] = '\0';
}
TCHAR device_id[device_id_max_length];
DWORD device_id_actual_length = 0;
BOOL got_device_id = SetupDiGetDeviceRegistryProperty(
device_info_set,
&device_info_data,
SPDRP_LOCATION_INFORMATION,
NULL,
(PBYTE)device_id,
device_id_max_length,
&device_id_actual_length);
if (got_device_id == TRUE && device_id_actual_length > 0) {
device_id[device_id_actual_length - 1] = '\0';
} else {
device_id[0] = '\0';
}
#ifdef UNICODE
std::string portName = utf8_encode(port_name);
std::string friendlyName = utf8_encode(friendly_name);
std::string hardwareId = utf8_encode(hardware_id);
std::string deviceId = utf8_encode(device_id);
#else
std::string portName = port_name;
std::string friendlyName = friendly_name;
std::string hardwareId = hardware_id;
std::string deviceId = device_id;
#endif
size_t pos = deviceId.find("#");
if (pos != std::string::npos) {
deviceId = deviceId.substr(pos + 1, 4);
deviceId = std::to_string(atoi(deviceId.c_str()));
}
if (hardwareId.find("VID_10C4&PID_EA60") != std::string::npos ||
hardwareId.find("VID_0483&PID_5740") != std::string::npos) {
PortInfo port_entry;
port_entry.port = portName;
port_entry.description = friendlyName;
port_entry.hardware_id = hardwareId;
port_entry.device_id = deviceId;
devices_found.push_back(port_entry);
}
}
SetupDiDestroyDeviceInfoList(device_info_set);
return devices_found;
}
#endif // #if defined(_WIN32)

10
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win.h Normal file
View File

@@ -0,0 +1,10 @@
#pragma once
#include <stddef.h>
#include <stdio.h>
#include <windows.h>
#include <stdlib.h>
#include <process.h>
#include <direct.h>

1077
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

175
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.h Normal file
View File

@@ -0,0 +1,175 @@
#if defined(_WIN32)
#ifndef SERIAL_IMPL_WINDOWS_H
#define SERIAL_IMPL_WINDOWS_H
#include <core/serial/serial.h>
#include <time.h>
#include <tchar.h>
#ifndef UNICODE
#define UNICODE
#define UNICODE_WAS_UNDEFINED
#endif
#include "windows.h"
#ifndef UNICODE_WAS_UNDEFINED
#undef UNICODE
#endif
namespace ydlidar {
namespace core {
namespace serial {
using std::string;
using std::wstring;
using std::invalid_argument;
class Serial::SerialImpl {
public:
explicit SerialImpl(const string &port,
unsigned long baudrate,
bytesize_t bytesize,
parity_t parity,
stopbits_t stopbits,
flowcontrol_t flowcontrol);
virtual ~SerialImpl();
bool open();
Serial::SerialPortError getSystemError(int systemErrorCode) const;
void close();
bool isOpen() const;
size_t available();
bool waitReadable(uint32_t timeout);
void waitByteTimes(size_t count);
int waitfordata(size_t data_count, uint32_t timeout, size_t *returned_size);
size_t read(uint8_t *buf, size_t size = 1);
size_t write(const uint8_t *data, size_t length);
void flush();
void flushInput();
void flushOutput();
void sendBreak(int duration);
bool setBreak(bool level);
bool setRTS(bool level);
bool setDTR(bool level);
bool waitForChange();
bool getCTS();
bool getDSR();
bool getRI();
bool getCD();
uint32_t getByteTime();
void setPort(const string &port);
string getPort() const;
void setTimeout(Timeout &timeout);
Timeout getTimeout() const;
bool setBaudrate(unsigned long baudrate);
unsigned long getBaudrate() const;
bool setBytesize(bytesize_t bytesize);
bytesize_t getBytesize() const;
bool setParity(parity_t parity);
parity_t getParity() const;
bool setStopbits(stopbits_t stopbits);
stopbits_t getStopbits() const;
bool setFlowcontrol(flowcontrol_t flowcontrol);
flowcontrol_t getFlowcontrol() const;
bool setDcb(DCB *dcb);
bool getDcb(DCB *dcb);
int readLock();
int readUnlock();
int writeLock();
int writeUnlock();
protected:
bool reconfigurePort();
public:
enum {
DEFAULT_RX_BUFFER_SIZE = 2048,
DEFAULT_TX_BUFFER_SIZE = 128,
};
private:
wstring port_; // Path to the file descriptor
HANDLE fd_;
OVERLAPPED _wait_o;
OVERLAPPED communicationOverlapped;
OVERLAPPED readCompletionOverlapped;
OVERLAPPED writeCompletionOverlapped;
DWORD originalEventMask;
DWORD triggeredEventMask;
COMMTIMEOUTS currentCommTimeouts;
COMMTIMEOUTS restoredCommTimeouts;
bool is_open_;
Timeout timeout_; // Timeout for read operations
unsigned long baudrate_; // Baudrate
uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte
parity_t parity_; // Parity
bytesize_t bytesize_; // Size of the bytes
stopbits_t stopbits_; // Stop Bits
flowcontrol_t flowcontrol_; // Flow Control
// Mutex used to lock the read functions
HANDLE read_mutex;
// Mutex used to lock the write functions
HANDLE write_mutex;
};
}//serial
}//core
}//ydlidar
#endif // SERIAL_IMPL_WINDOWS_H
#endif // if defined(_WIN32)

409
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.cpp Normal file
View File

@@ -0,0 +1,409 @@
#include <algorithm>
#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__)
# include <alloca.h>
#endif
#if defined (__MINGW32__)
# define alloca __builtin_alloca
#endif
#include "serial.h"
#include "common.h"
namespace ydlidar {
namespace core {
using namespace common;
namespace serial {
using std::min;
using std::numeric_limits;
using std::vector;
using std::size_t;
using std::string;
using serial::Serial;
using serial::bytesize_t;
using serial::parity_t;
using serial::stopbits_t;
using serial::flowcontrol_t;
class Serial::ScopedReadLock {
public:
explicit ScopedReadLock(Serial::SerialImpl *pimpl) : pimpl_(pimpl) {
this->pimpl_->readLock();
}
~ScopedReadLock() {
this->pimpl_->readUnlock();
}
private:
// Disable copy constructors
ScopedReadLock(const ScopedReadLock &);
const ScopedReadLock &operator=(ScopedReadLock);
Serial::SerialImpl *pimpl_;
};
class Serial::ScopedWriteLock {
public:
explicit ScopedWriteLock(Serial::SerialImpl *pimpl) : pimpl_(pimpl) {
this->pimpl_->writeLock();
}
~ScopedWriteLock() {
this->pimpl_->writeUnlock();
}
private:
// Disable copy constructors
ScopedWriteLock(const ScopedWriteLock &);
const ScopedWriteLock &operator=(ScopedWriteLock);
Serial::SerialImpl *pimpl_;
};
Serial::Serial(const string &port, uint32_t baudrate, serial::Timeout timeout,
bytesize_t bytesize, parity_t parity, stopbits_t stopbits,
flowcontrol_t flowcontrol)
: pimpl_(new SerialImpl(port, baudrate, bytesize, parity,
stopbits, flowcontrol)) {
pimpl_->setTimeout(timeout);
}
Serial::~Serial() {
delete pimpl_;
}
bool Serial::open() {
return pimpl_->open();
}
void Serial::closePort() {
pimpl_->close();
}
bool Serial::isOpen() {
return pimpl_->isOpen();
}
size_t Serial::available() {
return pimpl_->available();
}
bool Serial::waitReadable() {
serial::Timeout timeout(pimpl_->getTimeout());
return pimpl_->waitReadable(timeout.read_timeout_constant);
}
void Serial::waitByteTimes(size_t count) {
pimpl_->waitByteTimes(count);
}
Serial::SerialPortError Serial::getSystemError(int systemErrorCode) const {
return pimpl_->getSystemError(systemErrorCode);
}
int Serial::waitfordata(size_t data_count, uint32_t timeout,
size_t *returned_size) {
return pimpl_->waitfordata(data_count, timeout, returned_size);
}
size_t Serial::writeData(const uint8_t *data, size_t size) {
return write(data, size);
}
size_t Serial::readData(uint8_t *data, size_t size) {
return read(data, size);
}
const char *Serial::DescribeError(SerialPortError err) {
char const *errorString = "Unknown error";
switch (err) {
case Serial::NoError:
errorString = ("No error");
break;
case Serial::OpenError:
errorString = ("Device is already open");
break;
case Serial::NotOpenError:
errorString = ("Device is not open");
break;
case Serial::TimeoutError:
errorString = ("Operation timed out");
break;
case Serial::ReadError:
errorString = ("Error reading from device");
break;
case Serial::WriteError:
errorString = ("Error writing to device");
break;
case Serial::ResourceError:
errorString = ("Device disappeared from the system");
break;
default:
// an empty string will be interpreted as "Unknown error"
break;
}
return errorString;
}
size_t Serial::read_(uint8_t *buffer, size_t size) {
return this->pimpl_->read(buffer, size);
}
size_t Serial::read(uint8_t *buffer, size_t size) {
ScopedReadLock lock(this->pimpl_);
return this->pimpl_->read(buffer, size);
}
size_t Serial::read(std::vector<uint8_t> &buffer, size_t size) {
ScopedReadLock lock(this->pimpl_);
uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
size_t bytes_read = this->pimpl_->read(buffer_, size);
buffer.insert(buffer.end(), buffer_, buffer_ + bytes_read);
return bytes_read;
}
size_t Serial::read(std::string &buffer, size_t size) {
ScopedReadLock lock(this->pimpl_);
uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
size_t bytes_read = this->pimpl_->read(buffer_, size);
buffer.append(reinterpret_cast<const char *>(buffer_), bytes_read);
return bytes_read;
}
string Serial::readSize(size_t size) {
std::string buffer;
this->read(buffer, size);
return buffer;
}
size_t Serial::readline(string &buffer, size_t size, string eol) {
ScopedReadLock lock(this->pimpl_);
size_t eol_len = eol.length();
uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
size_t read_so_far = 0;
while (true) {
size_t bytes_read = this->read_(buffer_ + read_so_far, 1);
read_so_far += bytes_read;
if (bytes_read == 0) {
break; // Timeout occured on reading 1 byte
}
if (string(reinterpret_cast<const char *>(buffer_ + read_so_far - eol_len),
eol_len) == eol) {
break; // EOL found
}
if (read_so_far == size) {
break; // Reached the maximum read length
}
}
buffer.append(reinterpret_cast<const char *>(buffer_), read_so_far);
return read_so_far;
}
string Serial::readline(size_t size, string eol) {
std::string buffer;
this->readline(buffer, size, eol);
return buffer;
}
vector<string> Serial::readlines(size_t size, string eol) {
ScopedReadLock lock(this->pimpl_);
std::vector<std::string> lines;
size_t eol_len = eol.length();
uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
size_t read_so_far = 0;
size_t start_of_line = 0;
while (read_so_far < size) {
size_t bytes_read = this->read_(buffer_ + read_so_far, 1);
read_so_far += bytes_read;
if (bytes_read == 0) {
if (start_of_line != read_so_far) {
lines.push_back(string(reinterpret_cast<const char *>(buffer_ + start_of_line),
read_so_far - start_of_line));
}
break; // Timeout occured on reading 1 byte
}
if (string(reinterpret_cast<const char *>
(buffer_ + read_so_far - eol_len), eol_len) == eol) {
// EOL found
lines.push_back(string(reinterpret_cast<const char *>(buffer_ + start_of_line),
read_so_far - start_of_line));
start_of_line = read_so_far;
}
if (read_so_far == size) {
if (start_of_line != read_so_far) {
lines.push_back(string(reinterpret_cast<const char *>(buffer_ + start_of_line),
read_so_far - start_of_line));
}
break; // Reached the maximum read length
}
}
return lines;
}
size_t Serial::write(const string &data) {
ScopedWriteLock lock(this->pimpl_);
return this->write_(reinterpret_cast<const uint8_t *>(data.c_str()),
data.length());
}
size_t Serial::write(const std::vector<uint8_t> &data) {
ScopedWriteLock lock(this->pimpl_);
return this->write_(&data[0], data.size());
}
size_t Serial::write(const uint8_t *data, size_t size) {
ScopedWriteLock lock(this->pimpl_);
return this->write_(data, size);
}
size_t Serial::write_(const uint8_t *data, size_t length) {
return pimpl_->write(data, length);
}
void Serial::setPort(const string &port) {
ScopedReadLock rlock(this->pimpl_);
ScopedWriteLock wlock(this->pimpl_);
bool was_open = pimpl_->isOpen();
if (was_open) {
closePort();
}
pimpl_->setPort(port);
if (was_open) {
open();
}
}
string Serial::getPort() const {
return pimpl_->getPort();
}
void Serial::setTimeout(serial::Timeout &timeout) {
pimpl_->setTimeout(timeout);
}
serial::Timeout Serial::getTimeout() const {
return pimpl_->getTimeout();
}
bool Serial::setBaudrate(uint32_t baudrate) {
return pimpl_->setBaudrate(baudrate);
}
uint32_t Serial::getBaudrate() const {
return uint32_t(pimpl_->getBaudrate());
}
bool Serial::setBytesize(bytesize_t bytesize) {
return pimpl_->setBytesize(bytesize);
}
bytesize_t Serial::getBytesize() const {
return pimpl_->getBytesize();
}
bool Serial::setParity(parity_t parity) {
return pimpl_->setParity(parity);
}
parity_t Serial::getParity() const {
return pimpl_->getParity();
}
bool Serial::setStopbits(stopbits_t stopbits) {
return pimpl_->setStopbits(stopbits);
}
stopbits_t Serial::getStopbits() const {
return pimpl_->getStopbits();
}
bool Serial::setFlowcontrol(flowcontrol_t flowcontrol) {
return pimpl_->setFlowcontrol(flowcontrol);
}
flowcontrol_t Serial::getFlowcontrol() const {
return pimpl_->getFlowcontrol();
}
void Serial::flush() {
ScopedReadLock rlock(this->pimpl_);
ScopedWriteLock wlock(this->pimpl_);
pimpl_->flush();
}
void Serial::flushInput() {
ScopedReadLock lock(this->pimpl_);
pimpl_->flushInput();
}
void Serial::flushOutput() {
ScopedWriteLock lock(this->pimpl_);
pimpl_->flushOutput();
}
void Serial::sendBreak(int duration) {
pimpl_->sendBreak(duration);
}
bool Serial::setBreak(bool level) {
return pimpl_->setBreak(level);
}
bool Serial::setRTS(bool level) {
return pimpl_->setRTS(level);
}
bool Serial::setDTR(bool level) {
return pimpl_->setDTR(level);
}
bool Serial::waitForChange() {
return pimpl_->waitForChange();
}
bool Serial::getCTS() {
return pimpl_->getCTS();
}
bool Serial::getDSR() {
return pimpl_->getDSR();
}
bool Serial::getRI() {
return pimpl_->getRI();
}
bool Serial::getCD() {
return pimpl_->getCD();
}
int Serial::getByteTime() {
return pimpl_->getByteTime();
}
}
}
}

710
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.h Normal file
View File

@@ -0,0 +1,710 @@
#ifndef SERIAL_H
#define SERIAL_H
#include <limits>
#include <vector>
#include <string>
#include <cstring>
#include <sstream>
#include <core/base/v8stdint.h>
#include <core/common/ChannelDevice.h>
namespace ydlidar {
namespace core {
using namespace common;
namespace serial {
/*!
* Enumeration defines the possible bytesizes for the serial port.
*/
typedef enum {
fivebits = 5,
sixbits = 6,
sevenbits = 7,
eightbits = 8
} bytesize_t;
/*!
* Enumeration defines the possible parity types for the serial port.
*/
typedef enum {
parity_none = 0,
parity_odd = 1,
parity_even = 2,
parity_mark = 3,
parity_space = 4
} parity_t;
/*!
* Enumeration defines the possible stopbit types for the serial port.
*/
typedef enum {
stopbits_one = 1,
stopbits_two = 2,
stopbits_one_point_five
} stopbits_t;
/*!
* Enumeration defines the possible flowcontrol types for the serial port.
*/
typedef enum {
flowcontrol_none = 0,
flowcontrol_software,
flowcontrol_hardware
} flowcontrol_t;
/*!
* Structure for setting the timeout of the serial port, times are
* in milliseconds.
*
* In order to disable the interbyte timeout, set it to Timeout::max().
*/
struct Timeout {
#ifdef max
# undef max
#endif
static uint32_t max() {
return std::numeric_limits<uint32_t>::max();
}
/*!
* Convenience function to generate Timeout structs using a
* single absolute timeout.
*
* \param timeout A long that defines the time in milliseconds until a
* timeout occurs after a call to read or write is made.
*
* \return Timeout struct that represents this simple timeout provided.
*/
static Timeout simpleTimeout(uint32_t timeout) {
return Timeout(max(), timeout, 0, timeout, 0);
}
/*! Number of milliseconds between bytes received to timeout on. */
uint32_t inter_byte_timeout;
/*! A constant number of milliseconds to wait after calling read. */
uint32_t read_timeout_constant;
/*! A multiplier against the number of requested bytes to wait after
* calling read.
*/
uint32_t read_timeout_multiplier;
/*! A constant number of milliseconds to wait after calling write. */
uint32_t write_timeout_constant;
/*! A multiplier against the number of requested bytes to wait after
* calling write.
*/
uint32_t write_timeout_multiplier;
explicit Timeout(uint32_t inter_byte_timeout_ = 0,
uint32_t read_timeout_constant_ = 0,
uint32_t read_timeout_multiplier_ = 0,
uint32_t write_timeout_constant_ = 0,
uint32_t write_timeout_multiplier_ = 0)
: inter_byte_timeout(inter_byte_timeout_),
read_timeout_constant(read_timeout_constant_),
read_timeout_multiplier(read_timeout_multiplier_),
write_timeout_constant(write_timeout_constant_),
write_timeout_multiplier(write_timeout_multiplier_) {
}
};
/*!
* Class that provides a portable serial port interface.
*/
class Serial : public ChannelDevice {
public:
/// Defines all error codes handled by the CSimpleSocket class.
typedef enum {
NoError,
DeviceNotFoundError,
PermissionError,
OpenError,
ParityError,
FramingError,
BreakConditionError,
WriteError,
ReadError,
ResourceError,
UnsupportedOperationError,
UnknownError,
TimeoutError,
NotOpenError
} SerialPortError;
/*!
* Creates a Serial object and opens the port if a port is specified,
* otherwise it remains closed until serial::Serial::open is called.
*
* \param port A std::string containing the address of the serial port,
* which would be something like 'COM1' on Windows and '/dev/ttyS0'
* on Linux.
*
* \param baudrate An unsigned 32-bit integer that represents the baudrate
*
* \param timeout A serial::Timeout struct that defines the timeout
* conditions for the serial port. \see serial::Timeout
*
* \param bytesize Size of each byte in the serial transmission of data,
* default is eightbits, possible values are: fivebits, sixbits, sevenbits,
* eightbits
*
* \param parity Method of parity, default is parity_none, possible values
* are: parity_none, parity_odd, parity_even
*
* \param stopbits Number of stop bits used, default is stopbits_one,
* possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
*
* \param flowcontrol Type of flowcontrol used, default is
* flowcontrol_none, possible values are: flowcontrol_none,
* flowcontrol_software, flowcontrol_hardware
*
* \throw serial::PortNotOpenedException
* \throw serial::IOException
* \throw std::invalid_argument
*/
explicit Serial(const std::string &port = "",
uint32_t baudrate = 9600,
Timeout timeout = Timeout(),
bytesize_t bytesize = eightbits,
parity_t parity = parity_none,
stopbits_t stopbits = stopbits_one,
flowcontrol_t flowcontrol = flowcontrol_none);
/*! Destructor */
virtual ~Serial();
/*!
* Opens the serial port as long as the port is set and the port isn't
* already open.
*
* If the port is provided to the constructor then an explicit call to open
* is not needed.
*
* \see Serial::Serial
* \return Returns true if the port is open, false otherwise.
*/
virtual bool open();
/*! Gets the open status of the serial port.
*
* \return Returns true if the port is open, false otherwise.
*/
virtual bool isOpen();
/*! Closes the serial port. */
virtual void closePort();
/*! Return the number of characters in the buffer. */
virtual size_t available();
/*! Block until there is serial data to read or read_timeout_constant
* number of milliseconds have elapsed. The return value is true when
* the function exits with the port in a readable state, false otherwise
* (due to timeout or select interruption). */
bool waitReadable();
/*! Block for a period of time corresponding to the transmission time of
* count characters at present serial settings. This may be used in con-
* junction with waitReadable to read larger blocks of data from the
* port. */
void waitByteTimes(size_t count);
/**
* @brief getSystemError
* @param systemErrorCode
* @return
*/
Serial::SerialPortError getSystemError(int systemErrorCode = -1) const;
/**
* @brief Block until there is serial data to read or read_timeout_constant
* number of milliseconds have elapsed. The return value is greater than zero when
* the function exits with the serial port buffer is greater than or
* equal to data_count, false otherwise(due to timeout or select interruption).
* @param data_count A size_t that indicates how many bytes should be wait from
* the given serial port or network buffer.
* @param timeout waiting timeout time
* @param returned_size if it is not NULL, the actual number of bytes will be returned.
* @return A size_t representing the number of bytes wait as a result of the
* call to wait.
*/
virtual int waitfordata(size_t data_count, uint32_t timeout,
size_t *returned_size);
/*! Write a string to the serial port.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \param size A size_t that indicates how many bytes should be written from
* the given data buffer.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*/
virtual size_t writeData(const uint8_t *data, size_t size);
/*! Read a given amount of bytes from the serial port into a given buffer.
*
* The read function will return in one of three cases:
* * The number of requested bytes was read.
* * In this case the number of bytes requested will match the size_t
* returned by read.
* * A timeout occurred, in this case the number of bytes read will not
* match the amount requested, but no exception will be thrown. One of
* two possible timeouts occurred:
* * The inter byte timeout expired, this means that number of
* milliseconds elapsed between receiving bytes from the serial port
* exceeded the inter byte timeout.
* * The total timeout expired, which is calculated by multiplying the
* read timeout multiplier by the number of requested bytes and then
* added to the read timeout constant. If that total number of
* milliseconds elapses after the initial call to read a timeout will
* occur.
* * An exception occurred, in this case an actual exception will be thrown.
*
* \param buffer An uint8_t array of at least the requested size.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
*/
virtual size_t readData(uint8_t *data, size_t size);
/// Returns a human-readable description of the given error code
/// or the last error code of a socket
static const char *DescribeError(SerialPortError err);
///
/// \brief DescribeError
/// \return
///
virtual const char *DescribeError() {
return DescribeError(getSystemError());
};
/*! Read a given amount of bytes from the serial port into a given buffer.
*
* The read function will return in one of three cases:
* * The number of requested bytes was read.
* * In this case the number of bytes requested will match the size_t
* returned by read.
* * A timeout occurred, in this case the number of bytes read will not
* match the amount requested, but no exception will be thrown. One of
* two possible timeouts occurred:
* * The inter byte timeout expired, this means that number of
* milliseconds elapsed between receiving bytes from the serial port
* exceeded the inter byte timeout.
* * The total timeout expired, which is calculated by multiplying the
* read timeout multiplier by the number of requested bytes and then
* added to the read timeout constant. If that total number of
* milliseconds elapses after the initial call to read a timeout will
* occur.
* * An exception occurred, in this case an actual exception will be thrown.
*
* \param buffer An uint8_t array of at least the requested size.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
*/
size_t read(uint8_t *buffer, size_t size);
/*! Read a given amount of bytes from the serial port into a give buffer.
*
* \param buffer A reference to a std::vector of uint8_t.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
*/
size_t read(std::vector<uint8_t> &buffer, size_t size = 1);
/*! Read a given amount of bytes from the serial port into a give buffer.
*
* \param buffer A reference to a std::string.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
*/
size_t read(std::string &buffer, size_t size = 1);
/*! Read a given amount of bytes from the serial port and return a string
* containing the data.
*
* \param size A size_t defining how many bytes to be read.
*
* \return A std::string containing the data read from the port.
*
*/
virtual std::string readSize(size_t size = 1);
/*! Reads in a line or until a given delimiter has been processed.
*
* Reads from the serial port until a single line has been read.
*
* \param buffer A std::string reference used to store the data.
* \param size A maximum length of a line, defaults to 65536 (2^16)
* \param eol A string to match against for the EOL.
*
* \return A size_t representing the number of bytes read.
*
*/
size_t readline(std::string &buffer, size_t size = 65536,
std::string eol = "\n");
/*! Reads in a line or until a given delimiter has been processed.
*
* Reads from the serial port until a single line has been read.
*
* \param size A maximum length of a line, defaults to 65536 (2^16)
* \param eol A string to match against for the EOL.
*
* \return A std::string containing the line.
*
*/
std::string readline(size_t size = 65536, std::string eol = "\n");
/*! Reads in multiple lines until the serial port times out.
*
* This requires a timeout > 0 before it can be run. It will read until a
* timeout occurs and return a list of strings.
*
* \param size A maximum length of combined lines, defaults to 65536 (2^16)
*
* \param eol A string to match against for the EOL.
*
* \return A vector<string> containing the lines.
*
*/
std::vector<std::string> readlines(size_t size = 65536, std::string eol = "\n");
/*! Write a string to the serial port.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \param size A size_t that indicates how many bytes should be written from
* the given data buffer.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
* \throw serial::IOException
*/
size_t write(const uint8_t *data, size_t size);
/*! Write a string to the serial port.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*
*/
size_t write(const std::vector<uint8_t> &data);
/*! Write a string to the serial port.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*
*/
size_t write(const std::string &data);
/*! Sets the serial port identifier.
*
* \param port A const std::string reference containing the address of the
* serial port, which would be something like 'COM1' on Windows and
* '/dev/ttyS0' on Linux.
*
* \throw std::invalid_argument
*/
void setPort(const std::string &port);
/*! Gets the serial port identifier.
*
* \see Serial::setPort
*
* \throw std::invalid_argument
*/
std::string getPort() const;
/*! Sets the timeout for reads and writes using the Timeout struct.
*
* There are two timeout conditions described here:
* * The inter byte timeout:
* * The inter_byte_timeout component of serial::Timeout defines the
* maximum amount of time, in milliseconds, between receiving bytes on
* the serial port that can pass before a timeout occurs. Setting this
* to zero will prevent inter byte timeouts from occurring.
* * Total time timeout:
* * The constant and multiplier component of this timeout condition,
* for both read and write, are defined in serial::Timeout. This
* timeout occurs if the total time since the read or write call was
* made exceeds the specified time in milliseconds.
* * The limit is defined by multiplying the multiplier component by the
* number of requested bytes and adding that product to the constant
* component. In this way if you want a read call, for example, to
* timeout after exactly one second regardless of the number of bytes
* you asked for then set the read_timeout_constant component of
* serial::Timeout to 1000 and the read_timeout_multiplier to zero.
* This timeout condition can be used in conjunction with the inter
* byte timeout condition with out any problems, timeout will simply
* occur when one of the two timeout conditions is met. This allows
* users to have maximum control over the trade-off between
* responsiveness and efficiency.
*
* Read and write functions will return in one of three cases. When the
* reading or writing is complete, when a timeout occurs, or when an
* exception occurs.
*
* A timeout of 0 enables non-blocking mode.
*
* \param timeout A serial::Timeout struct containing the inter byte
* timeout, and the read and write timeout constants and multipliers.
*
* \see serial::Timeout
*/
void setTimeout(Timeout &timeout);
/*! Sets the timeout for reads and writes. */
void setTimeout(uint32_t inter_byte_timeout, uint32_t read_timeout_constant,
uint32_t read_timeout_multiplier, uint32_t write_timeout_constant,
uint32_t write_timeout_multiplier) {
Timeout timeout(inter_byte_timeout, read_timeout_constant,
read_timeout_multiplier, write_timeout_constant,
write_timeout_multiplier);
return setTimeout(timeout);
}
/*! Gets the timeout for reads in seconds.
*
* \return A Timeout struct containing the inter_byte_timeout, and read
* and write timeout constants and multipliers.
*
* \see Serial::setTimeout
*/
Timeout getTimeout() const;
/*! Sets the baudrate for the serial port.
*
* Possible baudrates depends on the system but some safe baudrates include:
* 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000,
* 57600, 115200
* Some other baudrates that are supported by some comports:
* 128000, 153600, 230400, 256000, 460800, 921600
*
* \param baudrate An integer that sets the baud rate for the serial port.
*
*/
bool setBaudrate(uint32_t baudrate);
/*! Gets the baudrate for the serial port.
*
* \return An integer that sets the baud rate for the serial port.
*
* \see Serial::setBaudrate
*
* \
*/
uint32_t getBaudrate() const;
/*! Sets the bytesize for the serial port.
*
* \param bytesize Size of each byte in the serial transmission of data,
* default is eightbits, possible values are: fivebits, sixbits, sevenbits,
* eightbits
*
* \
*/
bool setBytesize(bytesize_t bytesize);
/*! Gets the bytesize for the serial port.
*
* \see Serial::setBytesize
*
* \
*/
bytesize_t getBytesize() const;
/*! Sets the parity for the serial port.
*
* \param parity Method of parity, default is parity_none, possible values
* are: parity_none, parity_odd, parity_even
*
* \
*/
bool setParity(parity_t parity);
/*! Gets the parity for the serial port.
*
* \see Serial::setParity
*
* \
*/
parity_t getParity() const;
/*! Sets the stopbits for the serial port.
*
* \param stopbits Number of stop bits used, default is stopbits_one,
* possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
*
* \
*/
bool setStopbits(stopbits_t stopbits);
/*! Gets the stopbits for the serial port.
*
* \see Serial::setStopbits
*
* \
*/
stopbits_t getStopbits() const;
/*! Sets the flow control for the serial port.
*
* \param flowcontrol Type of flowcontrol used, default is flowcontrol_none,
* possible values are: flowcontrol_none, flowcontrol_software,
* flowcontrol_hardware
*
* \
*/
bool setFlowcontrol(flowcontrol_t flowcontrol);
/*! Gets the flow control for the serial port.
*
* \see Serial::setFlowcontrol
*
* \
*/
flowcontrol_t getFlowcontrol() const;
/*! Flush the input and output buffers */
void flush();
/*! Flush only the input buffer */
void flushInput();
/*! Flush only the output buffer */
void flushOutput();
/*! Sends the RS-232 break signal. See tcsendbreak(3). */
void sendBreak(int duration);
/*! Set the break condition to a given level. Defaults to true. */
bool setBreak(bool level = true);
/*! Set the RTS handshaking line to the given level. Defaults to true. */
bool setRTS(bool level = true);
/*! Set the DTR handshaking line to the given level. Defaults to true. */
virtual bool setDTR(bool level = true);
/*!
* Blocks until CTS, DSR, RI, CD changes or something interrupts it.
*
* Can throw an exception if an error occurs while waiting.
* You can check the status of CTS, DSR, RI, and CD once this returns.
* Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a
* resolution of less than +-1ms and as good as +-0.2ms. Otherwise a
* polling method is used which can give +-2ms.
*
* \return Returns true if one of the lines changed, false if something else
* occurred.
*
*/
bool waitForChange();
/*! Returns the current status of the CTS line. */
bool getCTS();
/*! Returns the current status of the DSR line. */
bool getDSR();
/*! Returns the current status of the RI line. */
bool getRI();
/*! Returns the current status of the CD line. */
bool getCD();
/*! Returns the singal byte time. */
virtual int getByteTime();
private:
// Disable copy constructors
Serial(const Serial &);
Serial &operator=(const Serial &);
// Pimpl idiom, d_pointer
class SerialImpl;
SerialImpl *pimpl_;
SerialPortError m_serialErrno; /// number of last error
// Scoped Lock Classes
class ScopedReadLock;
class ScopedWriteLock;
// Read common function
size_t read_(uint8_t *buffer, size_t size);
// Write common function
size_t write_(const uint8_t *data, size_t length);
/// Set internal socket error to that specified error
/// @param error type of error
void SetSerialPortError(SerialPortError error) {
m_serialErrno = error;
};
};
/*!
* Structure that describes a serial device.
*/
struct PortInfo {
/*! Address of the serial port (this can be passed to the constructor of Serial). */
std::string port;
/*! Human readable description of serial device if available. */
std::string description;
/*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */
std::string hardware_id;
/*! Hardware Device ID or "" if not available. */
std::string device_id;
};
/* Lists the serial ports available on the system
*
* Returns a vector of available serial ports, each represented
* by a serial::PortInfo data structure:
*
* \return vector of serial::PortInfo.
*/
std::vector<PortInfo>
list_ports();
} // namespace serial
}// namespace core
}// namespace ydlidar
#endif

13
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,13 @@
include_directories(..)
include_directories(.)
aux_include_directory(. HDRS)
aux_source_directory(. SRCS)
add_to_ydlidar_headers(${HDRS})
add_to_ydlidar_sources(${SRCS})
include_directories( ${CMAKE_CURRENT_SOURCE_DIR})
subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR})
foreach(subdir ${SUBDIRS})
include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/${subdir} )
add_subdirectory(${subdir})
endforeach()

1881
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

406
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.h Normal file
View File

@@ -0,0 +1,406 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019-2020 EAIBOT. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
/** @mainpage CYdLidar(YDLIDAR SDK API)
<table>
<tr><th>Library <td>CYdLidar
<tr><th>File <td>CYdLidar.h
<tr><th>Author <td>Tony [code at ydlidar com]
<tr><th>Source <td>https://github.com/ydlidar/YDLidar-SDK
<tr><th>Version <td>1.0.0
<tr><th>Sample <td>[ydlidar sample](\ref samples/ydlidar_test.cpp)[G1 G2 G4 G6 S2 X2 X4)\n
[tof sample](\ref samples/tof_test.cpp)[TG15 TG30 TG50 TX8 TX20]\n
[etlidar sample](\ref samples/etlidar_test.cpp)[T5 T15]
</table>
This API calls Two LiDAR interface classes in the following sections:
- @subpage YDlidarDriver
- @subpage ETLidarDriver
- @subpage YDLIDAR C API
* @copyright Copyright (c) 2018-2020 EAIBOT
Jump to the @link ::CYdLidar @endlink interface documentation.
*/
#ifndef CYDLIDAR_H
#define CYDLIDAR_H
#include <core/base/utils.h>
#include <core/common/ydlidar_def.h>
#include <core/common/DriverInterface.h>
#include <string>
#include <map>
class YDLIDAR_API CYdLidar {
public:
/**
* @brief create object
*/
CYdLidar();
/**
* @brief destroy object
*/
virtual ~CYdLidar();
/**
* @brief set lidar properties
* @param optname option name
* @param optval option value
* - std::string(or char*)
* - int
* - bool
* - float
* @param optlen option length
* - data type size
* @return true if the Property is set successfully, otherwise false.
* @see LidarProperty
*/
bool setlidaropt(int optname, const void *optval, int optlen);
/**
* @brief get lidar property
* @param optname option name
* @param optval option value
* - std::string(or char*)
* - int
* - bool
* - float
* @param optlen option length
* - data type size
* @return true if the Property is get successfully, otherwise false.
* @see LidarProperty
*/
bool getlidaropt(int optname, void *optval, int optlen);
/**
* @brief Initialize the SDK and LiDAR.
* @return true if successfully initialized, otherwise false.
*/
bool initialize();
/**
* @brief Return LiDAR's version information in a numeric form.
* @param version Pointer to a version structure for returning the version information.
*/
void GetLidarVersion(LidarVersion &version);
/**
* @brief Start the device scanning routine which runs on a separate thread and enable motor.
* @return true if successfully started, otherwise false.
*/
bool turnOn();
//判断是否已启动扫描
bool isScanning() const;
/**
* @brief Get the LiDAR Scan Data. turnOn is successful before doProcessSimple scan data.
* @param[out] outscan LiDAR Scan Data
* @param[out] hardwareError hardware error status
* @return true if successfully started, otherwise false.
*/
bool doProcessSimple(LaserScan &outscan);
/**
* @brief Stop the device scanning thread and disable motor.
* @return true if successfully Stoped, otherwise false.
*/
bool turnOff();
/**
* @brief Uninitialize the SDK and Disconnect the LiDAR.
*/
void disconnecting();
/**
* @brief Get the last error information of a (socket or serial)
* @return a human-readable description of the given error information
* or the last error information of a (socket or serial)
*/
const char *DescribeError() const;
/**
* @brief getDriverError
* @return
*/
DriverError getDriverError() const;
/**
* @brief 设置雷达工作模式（目前只针对GS2雷达）
* @param[in] mode 雷达工作模式
* @param[in] addr 雷达地址
* @return 成功返回true，否则返回false
*/
bool setWorkMode(int mode, uint8_t addr=0x00);
/**
* @brief 是否开启阳光噪点过滤功能
* @param[in] e true开启，false关闭
* @return 无
*/
void enableSunNoise(bool e=true);
/**
* @brief 是否开启玻璃噪点过滤功能
* @param[in] e true开启，false关闭
* @return 无
*/
void enableGlassNoise(bool e=true);
/**
* @brief 获取用户版本（目前只针对三角雷达）
* @param[out] version 用户版本
* @return 成功返回true，否则返回false
*/
bool getUserVersion(std::string &version);
//设置是否优先获取底板设备信息
void setBottomPriority(bool yes=true);
//获取设备信息
bool getDeviceInfo(device_info& di, int type);
//获取级联设备信息
bool getDeviceInfo(std::vector<device_info_ex>& dis);
//设置是否自动识别强度（启用时会占用一定时间）
void setAutoIntensity(bool yes=false);
//获取俯仰角值（目前仅针对Tmini Plus（森合））
bool getPitchAngle(float& pitch);
//启用调试
void setEnableDebug(bool yes) {m_Debug = yes;}
//OTA功能相关
//设置OTA文件路径
void setOtaFile(const std::string& name) {
otaName = name;
}
//设置OTA文件加密
void setOtaEncode(bool e) {
otaEncode = e;
}
//开始OTA升级
bool ota();
private:
/**
* @brief check LiDAR instance and connect to LiDAR,
* try to create a comms channel.
* @return true if communication has been established with the device.
* If it's not false on error.
*/
bool checkConnect();
/**
* @brief check LiDAR health state and device information
* @return true if health status and device information has been obtained with the device.
* If it's not, false on error
*/
bool checkStatus();
/**
* @brief check LiDAR scan state
* @return true if the normal scan runs with the device.
* If it's not, false on error.
*/
bool checkHardware();
/**
* @brief Get LiDAR Health state
* @return true if the device is in good health, If it's not
*/
bool getDeviceHealth();
/**
* @brief Get LiDAR Device information
* @return true if the device information is correct, If it's not
*/
bool getDeviceInfo();
/**
* @brief check LiDAR Scan frequency
* @return true if successfully checked, otherwise false.
*/
bool checkScanFrequency();
/**
* @brief checkHeartBeat
* @return
*/
bool checkHeartBeat();
/*!
* @brief check LiDAR sample rate
*/
void checkSampleRate();
/**
* @brief check LiDAR Data state
* @return true if LiDAR Data is Normal, otherwise false.
*/
bool checkLidarAbnormal();
/**
* @brief Calculate LiDAR Sampling rate
* @param count LiDAR Points
* @param scan_time LiDAR scan time
* @return true if successfully calculated, otherwise false.
*/
bool calcSampleRate(int count, double scan_time);
/**
* @brief Check if the LiDAR Offset Angle is corrected.
* @param serialNumber LiDAR serial number
*/
bool checkCalibrationAngle(const std::string &serialNumber);
/**
* @brief Whether the current LiDAR range is valid
* @param reading current LiDAR point range
* @return true if within valid range, otherwise false.
*/
bool isRangeValid(double reading) const;
/**
* @brief Whether the current LiDAR point is ignored
* @param angle current LiDAR point angle
* @return true if within ignore array, otherwise false.
*/
bool isRangeIgnore(double angle) const;
/**
* @brief handle single-channel LiDAR device information
* @note Start LiDAR successfully, handle single channel LiDAR
* Device Information
*/
void handleSingleChannelDevice();
/**
* @brief Parse Version by Package Information
* @param debug LiDAR Point CT Pakcage Information
*/
bool getDeviceInfoByPackage(const LaserDebug &debug);
/**
* @brief Calculate real-time sampling frequency
* @param frequency LiDAR current Scan Frequency
* @param count LiDAR Points
* @param tim_scan_end Last Scan Point Time Stamp
* @param tim_scan_start First Scan Point Time Stamp
*/
void resample(int frequency, int count, uint64_t tim_scan_end,
uint64_t tim_scan_start);
/**
* @brief Get zero correction angle
* @return zero correction angle
*/
float getAngleOffset() const;
/**
* @brief isAngleOffsetCorrected
* @return true if successfully corrected, otherwise false.
*/
bool isAngleOffsetCorrected() const;
private:
int m_FixedSize; ///< Fixed LiDAR Points
float m_AngleOffset; ///< Zero angle offset value
bool m_isAngleOffsetCorrected; ///< Has the Angle offset been corrected
float frequencyOffset; ///< Fixed Scan Frequency Offset
int lidar_model; ///< LiDAR Model
uint8_t Major; ///< Firmware Major Version
uint8_t Minjor; ///< Firmware Minjor Version
ydlidar::core::common::DriverInterface *lidarPtr; ///< LiDAR Driver Interface pointer
uint64_t m_PointTime; ///< Time interval between two sampling point
uint64_t last_node_time; ///< Latest LiDAR Start Node Time
node_info *global_nodes; ///< global nodes buffer
double last_frequency; ///< Latest Scan Frequency
uint64_t m_FristNodeTime; ///< Calculate real-time sample rate start time
uint64_t m_AllNode; ///< Sum of sampling points
std::map<int, int> SampleRateMap; ///< Sample Rate Map
std::string m_SerialNumber; ///< LiDAR serial number
// int defalutSampleRate; ///< LiDAR Default Sampling Rate
std::vector<int> defalutSampleRate; //默认采样率可能是多个值
float m_field_of_view; ///< LiDAR Field of View Angle.
LidarVersion m_LidarVersion; ///< LiDAR Version information
float zero_offset_angle_scale; ///< LiDAR Zero Offset Angle
private:
std::string m_SerialPort; ///< LiDAR serial port
std::string m_IgnoreString; ///< LiDAR ignore array string
std::vector<float> m_IgnoreArray; ///< LiDAR ignore array
bool m_FixedResolution; ///< LiDAR fixed angle resolution
bool m_Reversion; ///< LiDAR reversion
bool m_Inverted; ///< LiDAR inverted
bool m_AutoReconnect; ///< LiDAR hot plug
bool m_SingleChannel; ///< LiDAR single channel
bool m_Intensity; ///< LiDAR Intensity
int m_IntensityBit; ///< LiDAR Intensity bit
bool m_AutoIntensity; //自动识别强度
bool m_SupportMotorDtrCtrl; ///< LiDAR Motor DTR
bool m_SupportHearBeat; ///< LiDAR HeartBeat
int m_SerialBaudrate; ///< LiDAR serial baudrate or network port
int m_LidarType; ///< LiDAR type
int m_DeviceType; ///< LiDAR device type
float m_SampleRate; ///< LiDAR sample rate
int m_SampleRatebyD1; ///< LiDAR sample rate by d1
int m_AbnormalCheckCount; ///< LiDAR abnormal count
float m_MaxAngle; ///< LiDAR maximum angle
float m_MinAngle; ///< LiDAR minimum angle
float m_MaxRange; ///< LiDAR maximum range
float m_MinRange; ///< LiDAR minimum range
float m_ScanFrequency; ///< LiDAR scanning frequency
bool m_Bottom = true; //是否底板优先
bool m_Debug = false; //是否启用调试
bool m_SunNoise = false; //阳光噪点过滤标识
bool m_GlassNoise = false; //玻璃噪点过滤标识
std::string otaName; //OTA文件路径
bool otaEncode = true; //OTA是否加密
uint64_t lastStamp = 0; //时间戳
}; // End of class
#endif // CYDLIDAR_H
//os
namespace ydlidar
{
/**
* @brief system signal initialize
*/
YDLIDAR_API void os_init();
/**
* @brief Whether system signal is initialized.
* @return
*/
YDLIDAR_API bool os_isOk();
/**
* @brief shutdown system signal
*/
YDLIDAR_API void os_shutdown();
/**
* @brief lidarPortList
* @return
*/
YDLIDAR_API std::map<std::string, std::string> lidarPortList();
//打印logo字符
YDLIDAR_API void printLogo();
}

995
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.cpp Normal file
View File

@@ -0,0 +1,995 @@
#include <math.h>
#include <algorithm>
#include "DTSLidarDriver.h"
#include "core/serial/common.h"
#include "ydlidar_config.h"
using namespace impl;
namespace ydlidar
{
DTSLidarDriver::DTSLidarDriver()
: _serial(NULL)
{
//串口配置参数
isAutoReconnect = true;
isAutoconnting = false;
m_baudrate = 921600;
m_PointTime = 1e9 / 5000;
retryCount = 0;
m_SingleChannel = false;
m_LidarType = TYPE_SDM18;
nodeIndex = 0;
recvBuff = std::vector<uint8_t>(SDKDTSPCSSIZE, 0);
scan_node_count = 0;
scan_node_buf = new node_info[SDK_DTS_POINT_COUNT * 5];
}
DTSLidarDriver::~DTSLidarDriver()
{
m_isScanning = false;
isAutoReconnect = false;
_thread.join();
{
ScopedLocker l(_cmd_lock);
if (_serial)
{
if (_serial->isOpen())
{
_serial->flush();
_serial->closePort();
}
delete _serial;
_serial = NULL;
}
}
{
ScopedLocker l(_lock);
if (scan_node_buf)
{
delete[] scan_node_buf;
scan_node_buf = NULL;
}
scan_node_count = 0;
}
}
result_t DTSLidarDriver::connect(const char *port, uint32_t baudrate)
{
m_baudrate = baudrate;
m_port = string(port);
{
ScopedLocker l(_cmd_lock);
if (!_serial)
{
_serial = new serial::Serial(
m_port,
m_baudrate,
serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT));
}
if (!_serial->open())
{
return RESULT_FAIL;
}
m_isConnected = true;
}
stopScan();
//clearDTR();
return RESULT_OK;
}
void DTSLidarDriver::disconnect()
{
isAutoReconnect = false;
if (!m_isConnected)
return;
stop();
delay(10);
ScopedLocker l(_cmd_lock);
if (_serial)
{
if (_serial->isOpen())
{
_serial->closePort();
}
}
m_isConnected = false;
}
result_t DTSLidarDriver::stopScan(uint32_t timeout)
{
UNUSED(timeout);
result_t ans;
if (!m_isConnected)
return RESULT_FAIL;
ScopedLocker l(_cmd_lock);
if ((ans = sendCmd(SDK_CMD_STOPSCAN)) != RESULT_OK)
{
return ans;
}
if ((ans = waitResp(SDK_CMD_STOPSCAN, timeout)) != RESULT_OK)
{
return ans;
}
delay(10);
return RESULT_OK;
}
result_t DTSLidarDriver::stop()
{
if (isAutoconnting)
isAutoReconnect = false;
disableDataGrabbing();
stopScan();
flushSerial();
return RESULT_OK;
}
/*
* @brief 获取激光数据
* @param nodebuffer out: 激光点信息
* @param count in: 一圈激光点数
* @param timeout in: 超时时间
* @return
*/
result_t DTSLidarDriver::grabScanData(
node_info *nodebuffer,
size_t &count,
uint32_t timeout)
{
result_t ret = RESULT_FAIL;
switch (_dataEvent.wait(timeout))
{
case Event::EVENT_TIMEOUT:
count = 0;
ret = RESULT_TIMEOUT;
break;
case Event::EVENT_OK:
{
ScopedLocker l(_lock);
count = min(count, scan_node_count);
memcpy(nodebuffer, scan_node_buf, count * SDKNODESIZE);
scan_node_count = 0;
ret = RESULT_OK;
_dataEvent.set(false); //重置状态
break;
}
default:
count = 0;
ret = RESULT_FAIL;
break;
}
return ret;
}
/*
* @brief 等待扫描数据
* @param nodes out:存储节点信息的数组
* @param count out:节点信息数组的大小，传入时表示期望接收的节点数量，返回时表示实际接收的节点数量
* @param timeout in:超时时间（毫秒）
* @return result_t 操作结果，成功返回RESULT_OK，失败返回RESULT_FAIL
*/
result_t DTSLidarDriver::waitScanData(
node_info *nodes,
size_t &count,
uint32_t timeout)
{
result_t ret = RESULT_FAIL;
// 检查是否已连接
if (!m_isConnected)
{
count = 0;
return RESULT_FAIL;
}
size_t recvCount = 0;
uint32_t st = getms();
uint32_t wt = 0;
//循环等待接收节点数据
while ((wt = getms() - st) < timeout &&
recvCount < count)
{
node_info node;
memset(&node, 0, SDKNODESIZE);
//解包激光数据
ret = waitPackage(&node, timeout - wt);
if (!IS_OK(ret))
{
count = recvCount;
return ret;
}
//单点
nodes[recvCount++] = node;
if (recvCount == count)
return RESULT_OK;
}
count = recvCount;
return RESULT_FAIL;
}
//激光数据解析线程
int DTSLidarDriver::cacheScanData()
{
node_info local_buf[SDK_DTS_POINT_COUNT];
size_t count = SDK_DTS_POINT_COUNT;
result_t ret = RESULT_FAIL;
int timeout_count = 0;
retryCount = 0;
m_isScanning = true;
while (m_isScanning)
{
count = SDK_DTS_POINT_COUNT;
ret = waitScanData(local_buf, count);
//如果解析点云失败
if (!IS_OK(ret))
{
if (timeout_count > DEFAULT_TIMEOUT_COUNT)
{
if (!isAutoReconnect)
{
fprintf(stderr, "[YDLIDAR] Exit scanning thread!\n");
fflush(stderr);
m_isScanning = false;
return RESULT_FAIL;
}
else
{
ret = checkAutoConnecting();
if (IS_OK(ret))
{
timeout_count = 0;
}
else
{
m_isScanning = false;
return RESULT_FAIL;
}
}
}
else
{
timeout_count ++;
fprintf(stderr, "[YDLIDAR] Timeout count %d\n", timeout_count);
fflush(stderr);
}
}
else
{
timeout_count = 0;
retryCount = 0;
ScopedLocker l(_lock);
memcpy(scan_node_buf, local_buf, sizeof(node_info) * count);
scan_node_count = count;
_dataEvent.set();
}
}
m_isScanning = false;
return RESULT_OK;
}
/*
*@brief 创建解析雷达数据线程
*@note 创建解析雷达数据线程之前，必须使用startScan函数开启扫图成功
*/
result_t DTSLidarDriver::createThread()
{
_thread = CLASS_THREAD(DTSLidarDriver, cacheScanData);
if (!_thread.getHandle())
{
m_isScanning = false;
printf("[YDLIDAR] Fail to create DTS thread\n");
return RESULT_FAIL;
}
printf("[YDLIDAR] Create DTS thread [0x%X]\n", _thread.getHandle());
fflush(stdout);
m_isScanning = true;
return RESULT_OK;
}
result_t DTSLidarDriver::startScan(bool force, uint32_t timeout)
{
result_t ret = RESULT_FAIL;
if (!m_isConnected)
return RESULT_FAIL;
if (m_isScanning)
return RESULT_OK;
//启动前先停止
stopScan();
//设置默认扫描频率(无)
ret = setScanFreq(10.0, timeout);
if (!IS_OK(ret))
{
printf("[YDLIDAR] Fail to setting scan frequency\n");
return ret;
}
//获取校准参数
// ret = getCalibParam(timeout);
// if (!IS_OK(ret))
// {
// return ret;
// }
//发送启动雷达命令
ScopedLocker l(_cmd_lock);
if ((ret = sendCmd(SDK_CMD_STARTSCAN)) != RESULT_OK)
return ret;
//双通雷达才等待启动响应命令
if (!m_SingleChannel)
{
ret = waitResp(SDK_CMD_STARTSCAN, timeout);
if (ret != RESULT_OK)
{
printf("[YDLIDAR] Response to start scan error!\n");
return ret;
}
}
ret = createThread(); //创建线程
return ret;
}
std::string DTSLidarDriver::getSDKVersion()
{
return YDLIDAR_SDK_VERSION_STR;
}
const char *DTSLidarDriver::DescribeError(bool isTCP)
{
if (_serial)
{
return _serial->DescribeError();
}
return nullptr;
}
bool DTSLidarDriver::isscanning() const
{
return m_isScanning;
}
bool DTSLidarDriver::isconnected() const
{
return m_isConnected;
}
void DTSLidarDriver::setAutoReconnect(const bool &enable)
{
isAutoReconnect = enable;
}
result_t DTSLidarDriver::waitPackage(node_info *node, uint32_t timeout)
{
int pos = 0;
uint32_t st = getms();
uint32_t wt = 0;
uint16_t dataSize = 0; //data区数据的长度
uint16_t cs = 0; //CRC
result_t ret = RESULT_FAIL;
vector<uint8_t> crCdata;
memset(node, 0, SDKNODESIZE);
while ((wt = getms() - st) < timeout)
{
size_t srcSize = SDKDTSHEADSIZE - pos;
size_t dstSize = 0;
//dstSize为实际获取接收到的数据大小
result_t ans = waitForData(srcSize, timeout - wt, &dstSize);
if (!IS_OK(ans))
return ans;
//从串口中读取指定大小的数据
getData(recvBuff.data(), dstSize);
for (size_t i = 0; i < dstSize; ++i)
{
uint8_t c = recvBuff[i];
switch (pos)
{
case 0:
if (c != SDK_CMD_HEADFLAG)
{
pos = 0;
continue;
}
crCdata.clear();
break;
case 1:
if (c != SDK_DTS_DEVNUM)
{
pos = 0;
continue;
}
break;
case 2:
if (c != SDK_DTS_DEVTYPE)
{
pos = 0;
continue;
}
break;
case 3:
if (c != SDK_CMD_STARTSCAN) //判断解析到的命令字是否和指定命令字是否一致
{
pos = 0;
continue;
}
break;
case 4:
if (c != SDK_DTS_RESERVED) //判断解析到的命令字是否和指定命令字是否一致
{
pos = 0;
continue;
}
break;
case 5:
dataSize = uint16_t(c) << 8; //取出data区数据的长度
break;
case 6:
dataSize += uint16_t(c);
break;
default:
break;
}
pos ++;
crCdata.push_back(c);
}
//如果找到协议头
if (pos == SDKDTSHEADSIZE)
{
pos = 0;
//获取剩余数据，并计算校验码(data区数据长度 + CRC)
size_t srcSize = dataSize + 2;
size_t dstSize = 0;
//dstSize为实际获取接收到的数据大小
ret = waitForData(srcSize, timeout - wt, &dstSize);
if (!IS_OK(ret))
return ret;
//从串口中读取指定大小的数据
getData(recvBuff.data(), dstSize);
//存储数据区的数据(用于计算CRC)
for (size_t i = 0; i < dataSize; ++i)
{
crCdata.push_back(recvBuff[i]);
}
//计算校验码
cs = calculateCrc(crCdata);
//串口返回的CRC
uint16_t csRaw = (recvBuff[dataSize] << 8) | recvBuff[dataSize+1];
if (cs != csRaw)
{
printf("[YDLIDAR] CRC error calc[0x%04X] != src[0x%04X]\n",
cs, csRaw);
fflush(stdout);
return RESULT_FAIL;
}
break;
}
}
if (IS_OK(ret))
{
(*node).sync = NODE_SYNC;
(*node).stamp = getTime();
(*node).index = 0;
(*node).scanFreq = uint8_t(0);
(*node).qual = 0;
//取出主峰质心
uint16_t mainPeakQuality = (recvBuff[7] << 8) | recvBuff[6];
uint16_t qual = (recvBuff[11] << 8) | recvBuff[10];
(*node).qual = qual;
//主峰质心数据转十进制，再减去 b 值，然后再除以 k 值
(*node).dist = mainPeakQuality;
//这样是不是只有一个点
(*node).angle = 0;
//(*node).qual = 0;
(*node).is = 0;
return RESULT_OK;
}
else
{
return RESULT_FAIL;
}
}
result_t DTSLidarDriver::sendCmd(uint8_t cmd, const uint8_t *data, size_t dataSize)
{
if (!m_isConnected)
return RESULT_FAIL;
size_t size = SDKDTSHEADSIZE + dataSize;
vector<uint8_t> buff(size + 2, 0);
SdkDTSHead head;
head.head = SDK_CMD_HEADFLAG;
head.devNum = SDK_DTS_DEVNUM;
head.devType = SDK_DTS_DEVTYPE;
head.cmd = cmd;
head.reserved = SDK_DTS_RESERVED;
head.size = uint16_t(dataSize);
memcpy(&buff[0], &head, SDKDTSHEADSIZE);
if (data && dataSize)
memcpy(&buff[SDKDTSHEADSIZE], data, dataSize);
//CRC校验
uint16_t cs = calculateCrc(buff);
// buff[0] = static_cast<uint8_t>(cs >> 8); // 将高字节赋给下标为 0 的元素
// buff[1] = static_cast<uint8_t>(cs); // 将低字节赋给下标为 1 的元素
buff[size] = static_cast<uint8_t>(cs >> 8); //将高字节赋给下标为 0 的元素;
buff[size+1] = static_cast<uint8_t>(cs);// 将低字节赋给下标为 1 的元素
return sendData(buff.data(), buff.size());
}
result_t DTSLidarDriver::sendData(const uint8_t *data, size_t size)
{
if (!_serial || !_serial->isOpen())
return RESULT_FAIL;
if (!data || !size)
return RESULT_FAIL;
size_t r = 0;
while (size)
{
r = _serial->writeData(data, size);
if (!r)
return RESULT_FAIL;
if (m_Debug)
{
printf("send: ");
infoh(data, r);
}
size -= r;
data += r;
}
return RESULT_OK;
}
result_t DTSLidarDriver::waitResp(
uint8_t cmd, uint32_t timeout)
{
std::vector<uint8_t> data;
return waitResp(cmd, data, timeout);
}
/*
* @brief DTSLidarDriver::waitResp
* @param cmd in:命令字
* @param data out:接收到的数据
* @param timeout in：超时时间
* @return
*/
result_t DTSLidarDriver::waitResp(
uint8_t cmd,
std::vector<uint8_t> &data,
uint32_t timeout)
{
int pos = 0;
uint32_t st = getms();
uint32_t wt = 0;
vector<uint8_t> recvBuff(SDK_DTS_BUFFLEN, 0);
uint16_t cs = 0;
uint8_t dataSize = 0;
vector<uint8_t> crCdata;
while ((wt = getms() - st) < timeout)
{
size_t srcSize = SDKDTSHEADSIZE - pos;
size_t dstSize = 0;
//dstSize为实际获取接收到的数据大小
result_t ans = waitForData(srcSize, timeout - wt, &dstSize);
if (!IS_OK(ans))
return ans;
//从串口中读取指定大小的数据
getData(recvBuff.data(), dstSize);
for (size_t i = 0; i < dstSize; ++i)
{
uint8_t c = recvBuff[i];
switch (pos)
{
case 0:
if (c != SDK_CMD_HEADFLAG)
{
pos = 0;
continue;
}
crCdata.clear();
break;
case 1:
if (c != SDK_DTS_DEVNUM)
{
pos = 0;
continue;
}
break;
case 2:
if (c != SDK_DTS_DEVTYPE)
{
pos = 0;
continue;
}
break;
case 3:
if (c != cmd) //判断解析到的命令字是否和指定命令字是否一致
{
pos = 0;
continue;
}
break;
case 4:
if (c != SDK_DTS_RESERVED) //判断解析到的命令字是否和指定命令字是否一致
{
pos = 0;
continue;
}
break;
case 5:
dataSize = uint16_t(c) << 8; //取出data区数据的长度
break;
case 6:
dataSize += uint16_t(c);
break;
default:
break;
}
pos ++;
crCdata.push_back(c);
}
//如果找到协议头
if (pos == SDKDTSHEADSIZE)
{
pos = 0;
//获取剩余数据，并计算校验码(data区数据长度 + CRC)
size_t srcSize = dataSize + 2;
size_t dstSize = 0;
//dstSize为实际获取接收到的数据大小
result_t ans = waitForData(srcSize, timeout - wt, &dstSize);
if (!IS_OK(ans))
return ans;
//从串口中读取指定大小的数据
getData(recvBuff.data(), dstSize);
for (size_t i = 0; i < dataSize; ++i)
{
crCdata.push_back(recvBuff[i]);
data.push_back(recvBuff[i]); //数据存入输出参数
}
cs = calculateCrc(crCdata);
//串口返回的CRC
uint16_t csRaw = (recvBuff[dataSize] << 8) | recvBuff[dataSize+1];
//判断CRC是否一致
if (cs != csRaw)
{
printf("[YDLIDAR] CRC error calc[0x%04X] != src[0x%04X]\n",
cs, csRaw);
return RESULT_FAIL;
}
return RESULT_OK;
}
}
return RESULT_FAIL;
}
/*
* 等待数据函数
* @param srcSize in:期望接收的数据大小
* @param timeout in:超时时间
* @param dstSize out:实际接收到的数据大小
* @return 返回结果，表示等待数据的状态
*/
result_t DTSLidarDriver::waitForData(size_t srcSize, uint32_t timeout, size_t *dstSize)
{
//用于存储实际接收到的数据大小
size_t size = 0;
//如果dstSize为空指针，则将其指向size变量
if (!dstSize)
dstSize = &size;
//等待数据
result_t ret = _serial->waitfordata(srcSize, timeout, dstSize);
//如果实际接收到的数据大小大于期望大小，则将其截断为期望大小
if (IS_OK(ret))
{
if (*dstSize > srcSize)
*dstSize = srcSize;
}
return ret;
}
/*
* @brief 从串口中读取指定大小的数据
* @param data out:存储从串口读取的数据
* @param size in:指定需要读取的数据大小
* @return
*/
result_t DTSLidarDriver::getData(uint8_t *data, size_t size)
{
//检查串口是否打开
if (!_serial || !_serial->isOpen())
{
return RESULT_FAIL;
}
size_t r;
//循环读取数据，直到读取完指定的大小
while (size)
{
//从串口读取数据
r = _serial->readData(data, size);
//如果读取失败，则返回失败结果
if (!r)
{
return RESULT_FAIL;
}
//如果开启了调试模式，则打印读取的数据
if (m_Debug)
{
printf("recv: ");
infoh(data, r);
}
//更新剩余的数据大小和数据指针
size -= r;
data += r;
}
return RESULT_OK;
}
//设置扫描频率(无)
result_t DTSLidarDriver::setScanFreq(float sf, uint32_t timeout)
{
m_ScanFreq = 0;
return RESULT_OK;
}
result_t DTSLidarDriver::getCalibParam(uint32_t timeout)
{
ScopedLocker l(_cmd_lock);
if (sendCmd(SDK_CMD_CALIBPARAM) != RESULT_OK)
{
printf("[YDLIDAR] Fail to send CalibParam cmd\n");
return RESULT_FAIL;
}
vector<uint8_t> data;
if (waitResp(SDK_CMD_CALIBPARAM, data, timeout) != RESULT_OK)
{
printf("[YDLIDAR] Fail to get CalibParam\n");
return RESULT_FAIL;
}
//取出校准参数k,b的值
memcpy(&k, &data[7], sizeof(float));
memcpy(&b, &data[11], sizeof(float));
printf("[YDLIDAR] CalibParam k[%f] b[%f]\n", k, b);
// CalibParamInfo calibParamInfo;
// memcpy(&calibParamInfo, data.data(), sizeof(CalibParamInfo));
// k = calibParamInfo.k;
// b = calibParamInfo.b;
return RESULT_OK;
}
result_t DTSLidarDriver::checkAutoConnecting()
{
result_t ans = RESULT_FAIL;
isAutoconnting = true;
while (isAutoReconnect && isAutoconnting)
{
{
ScopedLocker l(_cmd_lock);
if (_serial)
{
if (_serial->isOpen() || m_isConnected)
{
m_isConnected = false;
_serial->closePort();
delete _serial;
_serial = nullptr;
}
}
}
retryCount ++;
if (retryCount > 10)
retryCount = 10;
int retryConnect = 0;
while (isAutoReconnect &&
connect(m_port.c_str(), m_baudrate) != RESULT_OK)
{
retryConnect ++;
if (retryConnect > 5)
{
retryConnect = 5;
}
setDriverError(NotOpenError);
delay(50 * retryConnect);
}
if (!isAutoReconnect)
{
m_isScanning = false;
return RESULT_FAIL;
}
if (isconnected())
{
delay(10);
{
ans = startAutoScan();
if (!IS_OK(ans))
{
ans = startAutoScan();
}
}
if (IS_OK(ans))
{
isAutoconnting = false;
return ans;
}
}
}
return RESULT_FAIL;
}
result_t DTSLidarDriver::startAutoScan(bool force, uint32_t timeout)
{
result_t ans;
if (!m_isConnected)
return RESULT_FAIL;
flushSerial();
delay(10);
{
ScopedLocker l(_cmd_lock);
if (sendCmd(SDK_CMD_STARTSCAN) != RESULT_OK)
{
return RESULT_FAIL;
}
if (!m_SingleChannel)
{
if ( waitResp(SDK_CMD_STARTSCAN, timeout) != RESULT_OK)
{
return RESULT_FAIL;
}
}
}
return RESULT_OK;
}
result_t DTSLidarDriver::getDeviceInfo(device_info &info, uint32_t timeout)
{
if (!m_isConnected)
return RESULT_FAIL;
ScopedLocker l(_cmd_lock);
if (sendCmd(SDK_CMD_CALIBPARAM) != RESULT_OK)
{
printf("[YDLIDAR] Fail to send CalibParam cmd\n");
return RESULT_FAIL;
}
vector<uint8_t> data;
if (waitResp(SDK_CMD_CALIBPARAM, data, timeout) != RESULT_OK)
{
printf("[YDLIDAR] Fail to get CalibParam\n");
return RESULT_FAIL;
}
// infoh(data.data(), data.size());
//取出校准参数k,b的值
// memcpy(&k, &data[0], sizeof(float));
// memcpy(&b, &data[4], sizeof(float));
// printf("[YDLIDAR] CalibParam k[%f] b[%f]\n", k, b);
memset(&info, 0, DEVICEINFOSIZE);
info.model = YDLIDAR_SDM18;
return RESULT_OK;
}
result_t DTSLidarDriver::getHealth(device_health &health, uint32_t timeout)
{
health.status = 0;
health.error_code = 0;
UNUSED(timeout);
return RESULT_OK;
}
const char *DTSLidarDriver::getErrorDesc(bool isTCP)
{
UNUSED(isTCP);
if (_serial)
{
return _serial->DescribeError();
}
return nullptr;
}
void DTSLidarDriver::disableDataGrabbing()
{
if (m_isScanning)
{
m_isScanning = false;
_dataEvent.set();
}
_thread.join();
}
void DTSLidarDriver::flushSerial()
{
if (!m_isConnected)
return;
ScopedLocker l(_cmd_lock);
size_t len = _serial->available();
if (len)
{
_serial->readSize(len);
}
delay(20);
}
uint16_t DTSLidarDriver::calculateCrc(const vector<uint8_t> &data)
{
uint16_t crc = 0xFFFF; //初始值为0xFFFF
for (const auto& byte : data)
{
crc ^= byte;
for (int i = 0; i < 8; i++)
{
if (crc & 0x0001)
{
crc = (crc >> 1) ^ 0xA001;
}
else
{
crc >>= 1;
}
}
}
return crc;
}
}

208
chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.h Normal file
View File

@@ -0,0 +1,208 @@
#ifndef DTSLIDARDRIVER_H
#define DTSLIDARDRIVER_H
#include <stdlib.h>
#include <atomic>
#include <map>
#include "core/serial/serial.h"
#include "core/base/locker.h"
#include "core/base/thread.h"
#include "core/common/ydlidar_protocol.h"
#include "core/common/ydlidar_help.h"
#if !defined(__cplusplus)
#ifndef __cplusplus
#error "The YDLIDAR SDK requires a C++ compiler to be built"
#endif
#endif
#define SDK_DTS_POINT_COUNT 1
#define SDK_CMD_HEADFLAG 0xA5 //协议头标识1
#define SDK_CMD_STARTSCAN 0x01 //开启测距
#define SDK_CMD_STOPSCAN 0x02 //停止测距
#define SDK_CMD_CALIBPARAM 0x06 //获取校准参数
#define SDK_DTS_DEVNUM 0x03 //设备号
#define SDK_DTS_DEVTYPE 0x20 //设备类型
#define SDK_DTS_RESERVED 0x00 //保留位
#define SDK_DTS_BUFFLEN 100
//设置1字节对齐
#pragma pack(1)
//DTS雷达协议头
struct SdkDTSHead
{
uint8_t head = 0; //包头
uint8_t devNum = 0; //设备号
uint8_t devType = 0; //设备类型
uint8_t cmd = 0; //命令功能码
uint8_t reserved = 0; //保留位
uint16_t size = 0; //数据大小
};
#define SDKDTSHEADSIZE sizeof(SdkDTSHead)
//DTS雷达单点数据
struct SdkDTSPc
{
uint16_t subPeakQuality = 0; //次峰质性
uint16_t tempCode = 0; //温度码
uint16_t subPeakIntensity = 0; //次峰强度
uint16_t mainPeakQuality = 0; //主峰质性
uint16_t mainPeakCalib = 0; //主峰校正
uint16_t mainPeakIntensity = 0; //主峰强度
uint16_t sunlitBase = 0; //阳光基底
};
//DTS雷达一包点云数据
struct SdkDTSPcs
{
SdkDTSHead head;
SdkDTSPc point; //一包数据只有单点
uint16_t cs = 0;//校验码
};
#define SDKDTSPCSSIZE sizeof(SdkDTSPcs)
//取消设置1字节对齐
#pragma pack()
//校准参数结构体
struct CalibParamInfo
{
float k = 0.0;
float b = 0.0;
char SN[9] = {0};
};
using namespace std;
namespace ydlidar
{
using namespace core;
using namespace core::serial;
using namespace core::base;
/*********************DTS雷达 ****************/
class DTSLidarDriver : public DriverInterface
{
public:
DTSLidarDriver();
virtual ~DTSLidarDriver();
result_t connect(const char *port, uint32_t baudrate);
void disconnect();
result_t stopScan(uint32_t timeout = DEFAULT_TIMEOUT / 2);
result_t stop();
/*
* @brief 获取激光数据
* @param nodebuffer out: 激光点信息
* @param count in: 一圈激光点数
* @param timeout in: 超时时间
* @return
*/
result_t grabScanData(node_info *nodebuffer, size_t &count,
uint32_t timeout = DEFAULT_TIMEOUT);
/*
* @brief 等待扫描数据
* @param nodes out:存储节点信息的数组
* @param count out:节点信息数组的大小，传入时表示期望接收的节点数量，返回时表示实际接收的节点数量
* @param timeout in:超时时间（毫秒）
* @return result_t 操作结果，成功返回RESULT_OK，失败返回RESULT_FAIL
*/
result_t waitScanData(node_info *nodes,
size_t &count,
uint32_t timeout = DEFAULT_TIMEOUT);
//激光数据解析线程
int cacheScanData();
result_t createThread();
result_t startScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT);
virtual std::string getSDKVersion();
virtual const char *DescribeError(bool isTCP = false);
//雷达是否处于扫图状态
bool isscanning() const;
//雷达是否处于连接状态
bool isconnected() const;
/*
* @brief 是否设置雷达异常自动重新连接
* @param enable in:是否开启自动重连
*/
void setAutoReconnect(const bool &enable);
/*
* @brief 解包激光数据 \n
* @param[in] node 解包后激光点信息
* @param[in] timeout 超时时间
*/
result_t waitPackage(node_info *node, uint32_t timeout = DEFAULT_TIMEOUT);
result_t sendCmd(uint8_t cmd,
const uint8_t *data = NULL,
size_t size = 0);
//串口发送数据
result_t sendData(const uint8_t *data, size_t size);
//等待响应
result_t waitResp(uint8_t cmd,
uint32_t timeout = DEFAULT_TIMEOUT);
/*
* @brief 等待数据(只获取响应data区的数据)
* @param cmd in:命令字
* @param data out:接收到的数据
* @param timeout in：超时时间
* @return
*/
result_t waitResp(uint8_t cmd,
std::vector<uint8_t> &data,
uint32_t timeout = DEFAULT_TIMEOUT);
/*
* 计算收到的数据的大小
* @param srcSize in:期望接收的数据大小
* @param timeout in:超时时间
* @param dstSize out:实际接收到的数据大小
* @return 返回结果，表示等待数据的状态
*/
result_t waitForData(size_t srcSize, uint32_t timeout = DEFAULT_TIMEOUT,
size_t *dstSize = NULL);
/*
* @brief 从串口中读取指定大小的数据
* @param data out:存储从串口读取的数据
* @param size in:指定需要读取的数据大小
* @return
*/
result_t getData(uint8_t *data, size_t size);
//设置扫描频率(无)
result_t setScanFreq(float sf, uint32_t timeout);
//获取校准参数
result_t getCalibParam(uint32_t timeout);
//自动连接
result_t checkAutoConnecting();
//重新连接开启扫描
result_t startAutoScan(bool force = false,
uint32_t timeout = DEFAULT_TIMEOUT);
//获取设备信息
virtual result_t getDeviceInfo(device_info &info,
uint32_t timeout = DEFAULT_TIMEOUT);
//获取健康状态
virtual result_t getHealth(device_health &health,
uint32_t timeout = DEFAULT_TIMEOUT);
//错误信息
virtual const char *getErrorDesc(bool isTCP = false);
//关闭数据获取通道
void disableDataGrabbing();
void flushSerial();
//CRC校验码(CRC-16/MODBUS)
uint16_t calculateCrc(const vector<uint8_t>& data);
private:
serial::Serial *_serial = nullptr; //串口
std::vector<uint8_t> recvBuff; //一包数据缓存
float k = 0; //校准参数k
float b = 0; //校准参数b
};
}
#endif // DTSLIDARDRIVER_H

Some files were not shown because too many files have changed in this diff Show More