From 2738b34dc1a62af3cac8908494b3b044420fbc05 Mon Sep 17 00:00:00 2001 From: fishros Date: Sat, 22 Feb 2025 20:45:21 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=9B=B4=E6=96=B0=E7=AC=AC=E4=B9=9D?= =?UTF-8?q?=E7=AB=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- chapt9/example_imu/.gitignore | 1 + .../example_imu/.vscode/c_cpp_properties.json | 513 +++ chapt9/example_imu/.vscode/extensions.json | 10 + chapt9/example_imu/.vscode/launch.json | 44 + chapt9/example_imu/include/README | 39 + chapt9/example_imu/lib/README | 46 + chapt9/example_imu/platformio.ini | 14 + chapt9/example_imu/src/main.cpp | 55 + chapt9/example_imu/test/README | 11 + chapt9/example_led/.gitignore | 1 + .../example_led/.vscode/c_cpp_properties.json | 511 +++ chapt9/example_led/.vscode/extensions.json | 10 + chapt9/example_led/.vscode/launch.json | 44 + chapt9/example_led/include/README | 39 + chapt9/example_led/lib/README | 46 + chapt9/example_led/platformio.ini | 13 + chapt9/example_led/src/main.cpp | 13 + chapt9/example_led/test/README | 11 + chapt9/example_project/.gitignore | 1 + .../.vscode/c_cpp_properties.json | 511 +++ .../example_project/.vscode/extensions.json | 10 + chapt9/example_project/.vscode/launch.json | 44 + chapt9/example_project/include/README | 39 + chapt9/example_project/lib/README | 46 + chapt9/example_project/platformio.ini | 13 + chapt9/example_project/src/main.cpp | 13 + chapt9/example_project/test/README | 11 + chapt9/example_ultrasound/.gitignore | 1 + .../.vscode/c_cpp_properties.json | 513 +++ .../.vscode/extensions.json | 10 + chapt9/example_ultrasound/.vscode/launch.json | 44 + chapt9/example_ultrasound/include/README | 39 + .../lib/PidController/PidController.cpp | 0 .../lib/PidController/PidController.h | 30 + chapt9/example_ultrasound/lib/README | 46 + chapt9/example_ultrasound/platformio.ini | 13 + chapt9/example_ultrasound/src/main.cpp | 24 + chapt9/example_ultrasound/test/README | 11 + chapt9/fishbot_motion_control/.gitignore | 1 + .../.vscode/c_cpp_properties.json | 531 +++ .../.vscode/extensions.json | 10 + .../.vscode/launch.json | 44 + .../.vscode/settings.json | 5 + chapt9/fishbot_motion_control/README.md | 4 + chapt9/fishbot_motion_control/include/README | 39 + .../lib/Kinematics/Kinematics.cpp | 89 + .../lib/Kinematics/Kinematics.h | 57 + .../lib/PidController/PidController.cpp | 64 + .../lib/PidController/PidController.h | 33 + chapt9/fishbot_motion_control/lib/README | 46 + chapt9/fishbot_motion_control/platformio.ini | 19 + chapt9/fishbot_motion_control/src/main.cpp | 152 + chapt9/fishbot_motion_control/test/9.4.1.cpp | 91 + chapt9/fishbot_motion_control/test/9.4.2.cpp | 108 + chapt9/fishbot_motion_control/test/9.4.3.cpp | 91 + chapt9/fishbot_motion_control/test/README | 11 + .../fishbot_motion_control_9.3.4/.gitignore | 1 + .../.vscode/c_cpp_properties.json | 519 +++ .../.vscode/extensions.json | 10 + .../.vscode/launch.json | 44 + .../.vscode/settings.json | 5 + chapt9/fishbot_motion_control_9.3.4/README.md | 4 + .../include/README | 39 + .../lib/Kinematics/Kinematics.cpp | 89 + .../lib/Kinematics/Kinematics.h | 57 + .../lib/PidController/PidController.cpp | 64 + .../lib/PidController/PidController.h | 33 + .../fishbot_motion_control_9.3.4/lib/README | 46 + .../platformio.ini | 16 + .../fishbot_motion_control_9.3.4/src/main.cpp | 52 + .../fishbot_motion_control_9.3.4/test/README | 11 + chapt9/fishbot_motion_control_9.3/.gitignore | 1 + .../.vscode/c_cpp_properties.json | 519 +++ .../.vscode/extensions.json | 10 + .../.vscode/launch.json | 44 + .../.vscode/settings.json | 5 + chapt9/fishbot_motion_control_9.3/README.md | 4 + .../fishbot_motion_control_9.3/include/README | 39 + .../lib/Kinematics/Kinematics.cpp | 89 + .../lib/Kinematics/Kinematics.h | 57 + .../lib/PidController/PidController.cpp | 64 + .../lib/PidController/PidController.h | 33 + chapt9/fishbot_motion_control_9.3/lib/README | 46 + .../fishbot_motion_control_9.3/platformio.ini | 16 + .../fishbot_motion_control_9.3/src/main.cpp | 54 + chapt9/fishbot_motion_control_9.3/test/README | 11 + .../fishbot_motion_control_9.4.1/.gitignore | 1 + .../.vscode/c_cpp_properties.json | 531 +++ .../.vscode/extensions.json | 10 + .../.vscode/launch.json | 44 + .../.vscode/settings.json | 5 + chapt9/fishbot_motion_control_9.4.1/README.md | 4 + .../include/README | 39 + .../lib/Kinematics/Kinematics.cpp | 89 + .../lib/Kinematics/Kinematics.h | 57 + .../lib/PidController/PidController.cpp | 64 + .../lib/PidController/PidController.h | 33 + .../fishbot_motion_control_9.4.1/lib/README | 46 + .../platformio.ini | 19 + .../fishbot_motion_control_9.4.1/src/main.cpp | 93 + .../fishbot_motion_control_9.4.1/test/README | 11 + .../src/fishbot_bringup/CMakeLists.txt | 8 +- .../fishbot_bringup/launch/bringup.launch.py | 1 + .../src/fishbot_bringup/map/room.pgm | Bin 0 -> 6763 bytes .../src/fishbot_bringup/map/room.yaml | 7 + .../src/fishbot_bringup/package.xml | 2 +- .../src/fishbot_description/CMakeLists.txt | 6 +- .../src/fishbot_description/package.xml | 2 +- .../src/fishbot_description/urdf/fishbot.urdf | 1 + .../src/fishbot_navigation2/CMakeLists.txt | 1 + .../config/nav2_params.yaml | 1 + .../src/fishbot_navigation2/maps/room.pgm | Bin 15671 -> 6763 bytes .../src/fishbot_navigation2/maps/room.yaml | 2 +- .../src/fishbot_navigation2/package.xml | 2 +- chapt9/fishbot_ws/src/micro-ROS-Agent | 1 + chapt9/fishbot_ws/src/micro_ros_msgs | 1 + .../fishbot_ws/src/ros_serial2wifi/.gitignore | 3 + .../src/ros_serial2wifi/package.xml | 18 + .../ros_serial2wifi/resource/ros_serail2wifi | 0 .../ros_serail2wifi/__init__.py | 0 .../ros_serail2wifi/tcpserver.py | 80 + .../fishbot_ws/src/ros_serial2wifi/setup.cfg | 4 + .../fishbot_ws/src/ros_serial2wifi/setup.py | 26 + .../ros_serial2wifi/test/test_copyright.py | 25 + .../src/ros_serial2wifi/test/test_flake8.py | 25 + .../src/ros_serial2wifi/test/test_pep257.py | 23 + chapt9/fishbot_ws/src/ydlidar_ros2/.gitignore | 3 + .../src/ydlidar_ros2/CMakeLists.txt | 137 + chapt9/fishbot_ws/src/ydlidar_ros2/README.md | 268 ++ .../ydlidar_ros2/cmake/FindPackage.cmake.in | 10 + .../ydlidar_ros2/cmake/PackageConfig.cmake.in | 24 + .../cmake/PackageConfigVersion.cmake.in | 17 + .../src/ydlidar_ros2/cmake/PkgConfig.pc.in | 11 + .../cmake/cmake_uninstall.cmake.in | 26 + .../cmake/common/ydlidar_base.cmake | 388 ++ .../cmake/common/ydlidar_parse.cmake | 71 + .../ydlidar_ros2/cmake/install_package.cmake | 296 ++ .../cmake/script_show_final_summary.cmake | 94 + .../src/ydlidar_ros2/config/ydlidar.rviz | 177 + .../src/ydlidar_ros2/docs/paramters.md | 69 + .../src/ydlidar_ros2/docs/ydlidar.md | 58 + .../src/ydlidar_ros2/launch/ydlidar.py | 29 + .../src/ydlidar_ros2/launch/ydlidar_launch.py | 49 + .../fishbot_ws/src/ydlidar_ros2/package.xml | 31 + .../src/ydlidar_ros2/params/ydlidar.yaml | 24 + .../src/ydlidar_ros2/sdk/CMakeLists.txt | 83 + .../src/ydlidar_ros2/sdk/core/CMakeLists.txt | 8 + .../ydlidar_ros2/sdk/core/base/CMakeLists.txt | 11 + .../src/ydlidar_ros2/sdk/core/base/datatype.h | 148 + .../src/ydlidar_ros2/sdk/core/base/locker.h | 359 ++ .../src/ydlidar_ros2/sdk/core/base/thread.h | 179 + .../src/ydlidar_ros2/sdk/core/base/timer.cpp | 57 + .../src/ydlidar_ros2/sdk/core/base/timer.h | 52 + .../src/ydlidar_ros2/sdk/core/base/typedef.h | 17 + .../src/ydlidar_ros2/sdk/core/base/utils.h | 18 + .../src/ydlidar_ros2/sdk/core/base/v8stdint.h | 144 + .../src/ydlidar_ros2/sdk/core/base/ydlidar.h | 194 + .../sdk/core/common/CMakeLists.txt | 5 + .../sdk/core/common/ChannelDevice.h | 134 + .../sdk/core/common/DriverInterface.h | 644 ++++ .../sdk/core/common/ydlidar_config.h | 28 + .../sdk/core/common/ydlidar_datatype.h | 121 + .../sdk/core/common/ydlidar_def.cpp | 39 + .../sdk/core/common/ydlidar_def.h | 246 ++ .../sdk/core/common/ydlidar_help.h | 1181 +++++++ .../sdk/core/common/ydlidar_protocol.h | 531 +++ .../ydlidar_ros2/sdk/core/json/CMakeLists.txt | 5 + .../src/ydlidar_ros2/sdk/core/json/cJSON.c | 3119 +++++++++++++++++ .../src/ydlidar_ros2/sdk/core/json/cJSON.h | 300 ++ .../ydlidar_ros2/sdk/core/math/CMakeLists.txt | 5 + .../src/ydlidar_ros2/sdk/core/math/angles.h | 297 ++ .../sdk/core/network/ActiveSocket.cpp | 302 ++ .../sdk/core/network/ActiveSocket.h | 100 + .../sdk/core/network/CMakeLists.txt | 10 + .../sdk/core/network/PassiveSocket.cpp | 315 ++ .../sdk/core/network/PassiveSocket.h | 128 + .../sdk/core/network/SimpleSocket.cpp | 1514 ++++++++ .../sdk/core/network/SimpleSocket.h | 691 ++++ .../ydlidar_ros2/sdk/core/network/StatTimer.h | 150 + .../sdk/core/serial/CMakeLists.txt | 10 + .../src/ydlidar_ros2/sdk/core/serial/common.h | 48 + .../sdk/core/serial/impl/CMakeLists.txt | 8 + .../sdk/core/serial/impl/unix/CMakeLists.txt | 4 + .../serial/impl/unix/list_ports_linux.cpp | 331 ++ .../sdk/core/serial/impl/unix/lock.c | 796 +++++ .../sdk/core/serial/impl/unix/lock.h | 241 ++ .../sdk/core/serial/impl/unix/unix.h | 27 + .../sdk/core/serial/impl/unix/unix_serial.cpp | 1634 +++++++++ .../sdk/core/serial/impl/unix/unix_serial.h | 162 + .../core/serial/impl/windows/CMakeLists.txt | 5 + .../serial/impl/windows/list_ports_win.cpp | 196 ++ .../sdk/core/serial/impl/windows/win.h | 10 + .../core/serial/impl/windows/win_serial.cpp | 1077 ++++++ .../sdk/core/serial/impl/windows/win_serial.h | 175 + .../ydlidar_ros2/sdk/core/serial/serial.cpp | 409 +++ .../src/ydlidar_ros2/sdk/core/serial/serial.h | 710 ++++ .../src/ydlidar_ros2/sdk/src/CMakeLists.txt | 13 + .../src/ydlidar_ros2/sdk/src/CYdLidar.cpp | 1881 ++++++++++ .../src/ydlidar_ros2/sdk/src/CYdLidar.h | 406 +++ .../ydlidar_ros2/sdk/src/DTSLidarDriver.cpp | 995 ++++++ .../src/ydlidar_ros2/sdk/src/DTSLidarDriver.h | 208 ++ .../ydlidar_ros2/sdk/src/ETLidarDriver.cpp | 1278 +++++++ .../src/ydlidar_ros2/sdk/src/ETLidarDriver.h | 454 +++ .../ydlidar_ros2/sdk/src/GSLidarDriver.cpp | 2030 +++++++++++ .../src/ydlidar_ros2/sdk/src/GSLidarDriver.h | 520 +++ .../ydlidar_ros2/sdk/src/SDMLidarDriver.cpp | 999 ++++++ .../src/ydlidar_ros2/sdk/src/SDMLidarDriver.h | 453 +++ .../ydlidar_ros2/sdk/src/TiaLidarDriver.cpp | 930 +++++ .../src/ydlidar_ros2/sdk/src/TiaLidarDriver.h | 248 ++ .../ydlidar_ros2/sdk/src/YDlidarDriver.cpp | 2808 +++++++++++++++ .../src/ydlidar_ros2/sdk/src/YDlidarDriver.h | 654 ++++ .../sdk/src/filters/CMakeLists.txt | 4 + .../sdk/src/filters/FilterInterface.h | 35 + .../sdk/src/filters/NoiseFilter.cpp | 601 ++++ .../sdk/src/filters/NoiseFilter.h | 107 + .../sdk/src/filters/StrongLightFilter.cpp | 214 ++ .../sdk/src/filters/StrongLightFilter.h | 56 + .../src/ydlidar_ros2/sdk/src/ydlidar_sdk.cpp | 232 ++ .../src/ydlidar_ros2/sdk/src/ydlidar_sdk.h | 265 ++ .../src/ydlidar_ros2/sdk/ydlidar_config.h.in | 28 + .../src/ydlidar_ros2/src/ydlidar_client.cpp | 42 + .../src/ydlidar_ros2/src/ydlidar_node.cpp | 263 ++ .../src/ydlidar_ros2_driver_client.cpp | 42 + .../src/ydlidar_ros2_driver_node.cpp | 263 ++ .../src/ydlidar_ros2/startup/initenv.sh | 11 + 225 files changed, 42292 insertions(+), 10 deletions(-) create mode 100644 chapt9/example_imu/.gitignore create mode 100644 chapt9/example_imu/.vscode/c_cpp_properties.json create mode 100644 chapt9/example_imu/.vscode/extensions.json create mode 100644 chapt9/example_imu/.vscode/launch.json create mode 100644 chapt9/example_imu/include/README create mode 100644 chapt9/example_imu/lib/README create mode 100644 chapt9/example_imu/platformio.ini create mode 100644 chapt9/example_imu/src/main.cpp create mode 100644 chapt9/example_imu/test/README create mode 100644 chapt9/example_led/.gitignore create mode 100644 chapt9/example_led/.vscode/c_cpp_properties.json create mode 100644 chapt9/example_led/.vscode/extensions.json create mode 100644 chapt9/example_led/.vscode/launch.json create mode 100644 chapt9/example_led/include/README create mode 100644 chapt9/example_led/lib/README create mode 100644 chapt9/example_led/platformio.ini create mode 100644 chapt9/example_led/src/main.cpp create mode 100644 chapt9/example_led/test/README create mode 100644 chapt9/example_project/.gitignore create mode 100644 chapt9/example_project/.vscode/c_cpp_properties.json create mode 100644 chapt9/example_project/.vscode/extensions.json create mode 100644 chapt9/example_project/.vscode/launch.json create mode 100644 chapt9/example_project/include/README create mode 100644 chapt9/example_project/lib/README create mode 100644 chapt9/example_project/platformio.ini create mode 100644 chapt9/example_project/src/main.cpp create mode 100644 chapt9/example_project/test/README create mode 100644 chapt9/example_ultrasound/.gitignore create mode 100644 chapt9/example_ultrasound/.vscode/c_cpp_properties.json create mode 100644 chapt9/example_ultrasound/.vscode/extensions.json create mode 100644 chapt9/example_ultrasound/.vscode/launch.json create mode 100644 chapt9/example_ultrasound/include/README create mode 100644 chapt9/example_ultrasound/lib/PidController/PidController.cpp create mode 100644 chapt9/example_ultrasound/lib/PidController/PidController.h create mode 100644 chapt9/example_ultrasound/lib/README create mode 100644 chapt9/example_ultrasound/platformio.ini create mode 100644 chapt9/example_ultrasound/src/main.cpp create mode 100644 chapt9/example_ultrasound/test/README create mode 100644 chapt9/fishbot_motion_control/.gitignore create mode 100644 chapt9/fishbot_motion_control/.vscode/c_cpp_properties.json create mode 100644 chapt9/fishbot_motion_control/.vscode/extensions.json create mode 100644 chapt9/fishbot_motion_control/.vscode/launch.json create mode 100644 chapt9/fishbot_motion_control/.vscode/settings.json create mode 100644 chapt9/fishbot_motion_control/README.md create mode 100644 chapt9/fishbot_motion_control/include/README create mode 100644 chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.cpp create mode 100644 chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.h create mode 100644 chapt9/fishbot_motion_control/lib/PidController/PidController.cpp create mode 100644 chapt9/fishbot_motion_control/lib/PidController/PidController.h create mode 100644 chapt9/fishbot_motion_control/lib/README create mode 100644 chapt9/fishbot_motion_control/platformio.ini create mode 100644 chapt9/fishbot_motion_control/src/main.cpp create mode 100644 chapt9/fishbot_motion_control/test/9.4.1.cpp create mode 100644 chapt9/fishbot_motion_control/test/9.4.2.cpp create mode 100644 chapt9/fishbot_motion_control/test/9.4.3.cpp create mode 100644 chapt9/fishbot_motion_control/test/README create mode 100644 chapt9/fishbot_motion_control_9.3.4/.gitignore create mode 100644 chapt9/fishbot_motion_control_9.3.4/.vscode/c_cpp_properties.json create mode 100644 chapt9/fishbot_motion_control_9.3.4/.vscode/extensions.json create mode 100644 chapt9/fishbot_motion_control_9.3.4/.vscode/launch.json create mode 100644 chapt9/fishbot_motion_control_9.3.4/.vscode/settings.json create mode 100644 chapt9/fishbot_motion_control_9.3.4/README.md create mode 100644 chapt9/fishbot_motion_control_9.3.4/include/README create mode 100644 chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.cpp create mode 100644 chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.h create mode 100644 chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.cpp create mode 100644 chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.h create mode 100644 chapt9/fishbot_motion_control_9.3.4/lib/README create mode 100644 chapt9/fishbot_motion_control_9.3.4/platformio.ini create mode 100644 chapt9/fishbot_motion_control_9.3.4/src/main.cpp create mode 100644 chapt9/fishbot_motion_control_9.3.4/test/README create mode 100644 chapt9/fishbot_motion_control_9.3/.gitignore create mode 100644 chapt9/fishbot_motion_control_9.3/.vscode/c_cpp_properties.json create mode 100644 chapt9/fishbot_motion_control_9.3/.vscode/extensions.json create mode 100644 chapt9/fishbot_motion_control_9.3/.vscode/launch.json create mode 100644 chapt9/fishbot_motion_control_9.3/.vscode/settings.json create mode 100644 chapt9/fishbot_motion_control_9.3/README.md create mode 100644 chapt9/fishbot_motion_control_9.3/include/README create mode 100644 chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.cpp create mode 100644 chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.h create mode 100644 chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.cpp create mode 100644 chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.h create mode 100644 chapt9/fishbot_motion_control_9.3/lib/README create mode 100644 chapt9/fishbot_motion_control_9.3/platformio.ini create mode 100644 chapt9/fishbot_motion_control_9.3/src/main.cpp create mode 100644 chapt9/fishbot_motion_control_9.3/test/README create mode 100644 chapt9/fishbot_motion_control_9.4.1/.gitignore create mode 100644 chapt9/fishbot_motion_control_9.4.1/.vscode/c_cpp_properties.json create mode 100644 chapt9/fishbot_motion_control_9.4.1/.vscode/extensions.json create mode 100644 chapt9/fishbot_motion_control_9.4.1/.vscode/launch.json create mode 100644 chapt9/fishbot_motion_control_9.4.1/.vscode/settings.json create mode 100644 chapt9/fishbot_motion_control_9.4.1/README.md create mode 100644 chapt9/fishbot_motion_control_9.4.1/include/README create mode 100644 chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.cpp create mode 100644 chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.h create mode 100644 chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.cpp create mode 100644 chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.h create mode 100644 chapt9/fishbot_motion_control_9.4.1/lib/README create mode 100644 chapt9/fishbot_motion_control_9.4.1/platformio.ini create mode 100644 chapt9/fishbot_motion_control_9.4.1/src/main.cpp create mode 100644 chapt9/fishbot_motion_control_9.4.1/test/README create mode 100644 chapt9/fishbot_ws/src/fishbot_bringup/map/room.pgm create mode 100644 chapt9/fishbot_ws/src/fishbot_bringup/map/room.yaml create mode 160000 chapt9/fishbot_ws/src/micro-ROS-Agent create mode 160000 chapt9/fishbot_ws/src/micro_ros_msgs create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/.gitignore create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/package.xml create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/resource/ros_serail2wifi create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/__init__.py create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/tcpserver.py create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/setup.cfg create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/setup.py create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/test/test_copyright.py create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/test/test_flake8.py create mode 100644 chapt9/fishbot_ws/src/ros_serial2wifi/test/test_pep257.py create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/.gitignore create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/README.md create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/FindPackage.cmake.in create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfig.cmake.in create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfigVersion.cmake.in create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PkgConfig.pc.in create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/cmake_uninstall.cmake.in create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_base.cmake create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_parse.cmake create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/install_package.cmake create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/cmake/script_show_final_summary.cmake create mode 100755 chapt9/fishbot_ws/src/ydlidar_ros2/config/ydlidar.rviz create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/docs/paramters.md create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/docs/ydlidar.md create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar.py create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar_launch.py create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/package.xml create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/params/ydlidar.yaml create mode 100755 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/datatype.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/locker.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/thread.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/typedef.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/utils.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/v8stdint.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/ydlidar.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ChannelDevice.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/DriverInterface.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_config.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_datatype.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_help.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_protocol.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.c create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/angles.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/StatTimer.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/common.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/list_ports_linux.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.c create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/list_ports_win.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.h create mode 100755 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.cpp create mode 100755 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/CMakeLists.txt create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/FilterInterface.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.h create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/sdk/ydlidar_config.h.in create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_client.cpp create mode 100644 chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_node.cpp create mode 100755 chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_client.cpp create mode 100755 chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_node.cpp create mode 100755 chapt9/fishbot_ws/src/ydlidar_ros2/startup/initenv.sh diff --git a/chapt9/example_imu/.gitignore b/chapt9/example_imu/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/example_imu/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/example_imu/.vscode/c_cpp_properties.json b/chapt9/example_imu/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..7b92baf --- /dev/null +++ b/chapt9/example_imu/.vscode/c_cpp_properties.json @@ -0,0 +1,513 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/example_imu/include", + "/home/fishros/chapt9/example_imu/src", + "/home/fishros/chapt9/example_imu/.pio/libdeps/fishbot/MPU6050_light/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/example_imu/include", + "/home/fishros/chapt9/example_imu/src", + "/home/fishros/chapt9/example_imu/.pio/libdeps/fishbot/MPU6050_light/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60116", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/example_imu/.vscode/extensions.json b/chapt9/example_imu/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/example_imu/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/example_imu/.vscode/launch.json b/chapt9/example_imu/.vscode/launch.json new file mode 100644 index 0000000..f0ff242 --- /dev/null +++ b/chapt9/example_imu/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/example_imu/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/example_imu/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/example_imu/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/example_imu/include/README b/chapt9/example_imu/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/example_imu/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/example_imu/lib/README b/chapt9/example_imu/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/example_imu/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/example_imu/platformio.ini b/chapt9/example_imu/platformio.ini new file mode 100644 index 0000000..aa812aa --- /dev/null +++ b/chapt9/example_imu/platformio.ini @@ -0,0 +1,14 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino +lib_deps = http://github.fishros.org/https://github.com/fishros/MPU6050_light.git \ No newline at end of file diff --git a/chapt9/example_imu/src/main.cpp b/chapt9/example_imu/src/main.cpp new file mode 100644 index 0000000..0c79af5 --- /dev/null +++ b/chapt9/example_imu/src/main.cpp @@ -0,0 +1,55 @@ +/* Get all possible data from MPU6050 + * Accelerometer values are given as multiple of the gravity [1g = 9.81 m/s²] + * Gyro values are given in deg/s + * Angles are given in degrees + * Note that X and Y are tilt angles and not pitch/roll. + * + * License: MIT + */ + +#include "Wire.h" +#include + +MPU6050 mpu(Wire); + +void setup() { + Serial.begin(115200); + Wire.begin(18,19); // sda, scl + + byte status = mpu.begin(); + Serial.print(F("MPU6050 status: ")); + Serial.println(status); + while(status!=0){ } // stop everything if could not connect to MPU6050 + + Serial.println(F("Calculating offsets, do not move MPU6050")); + delay(1000); + mpu.calcOffsets(true,true); // gyro and accelero + Serial.println("Done!\n"); +} + +unsigned long timer = 0; + +void loop() { + mpu.update(); + + if(millis() - timer > 1000){ // print data every second + Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp()); + Serial.print(F("ACCELERO X: "));Serial.print(mpu.getAccX()); + Serial.print("\tY: ");Serial.print(mpu.getAccY()); + Serial.print("\tZ: ");Serial.println(mpu.getAccZ()); + + Serial.print(F("GYRO X: "));Serial.print(mpu.getGyroX()); + Serial.print("\tY: ");Serial.print(mpu.getGyroY()); + Serial.print("\tZ: ");Serial.println(mpu.getGyroZ()); + + Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX()); + Serial.print("\tY: ");Serial.println(mpu.getAccAngleY()); + + Serial.print(F("ANGLE X: "));Serial.print(mpu.getAngleX()); + Serial.print("\tY: ");Serial.print(mpu.getAngleY()); + Serial.print("\tZ: ");Serial.println(mpu.getAngleZ()); + Serial.println(F("=====================================================\n")); + timer = millis(); + } + +} diff --git a/chapt9/example_imu/test/README b/chapt9/example_imu/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/example_imu/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/example_led/.gitignore b/chapt9/example_led/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/example_led/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/example_led/.vscode/c_cpp_properties.json b/chapt9/example_led/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..84b133c --- /dev/null +++ b/chapt9/example_led/.vscode/c_cpp_properties.json @@ -0,0 +1,511 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/example_led/include", + "/home/fishros/chapt9/example_led/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/example_led/include", + "/home/fishros/chapt9/example_led/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60116", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/example_led/.vscode/extensions.json b/chapt9/example_led/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/example_led/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/example_led/.vscode/launch.json b/chapt9/example_led/.vscode/launch.json new file mode 100644 index 0000000..1a3b907 --- /dev/null +++ b/chapt9/example_led/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/example_led/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/example_led/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/example_led/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/example_led/include/README b/chapt9/example_led/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/example_led/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/example_led/lib/README b/chapt9/example_led/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/example_led/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/example_led/platformio.ini b/chapt9/example_led/platformio.ini new file mode 100644 index 0000000..acc0f71 --- /dev/null +++ b/chapt9/example_led/platformio.ini @@ -0,0 +1,13 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino \ No newline at end of file diff --git a/chapt9/example_led/src/main.cpp b/chapt9/example_led/src/main.cpp new file mode 100644 index 0000000..1180c2a --- /dev/null +++ b/chapt9/example_led/src/main.cpp @@ -0,0 +1,13 @@ +#include +void setup() +{ + pinMode(2, OUTPUT); // 设置2号引脚模式为OUTPUT模式 +} +void loop() +{ + digitalWrite(2, LOW); // 低电平，打开LED灯 + delay(100); + // 休眠1000ms + digitalWrite(2, HIGH); // 高电平，关闭LED灯 + delay(100); +} \ No newline at end of file diff --git a/chapt9/example_led/test/README b/chapt9/example_led/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/example_led/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/example_project/.gitignore b/chapt9/example_project/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/example_project/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/example_project/.vscode/c_cpp_properties.json b/chapt9/example_project/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..1580738 --- /dev/null +++ b/chapt9/example_project/.vscode/c_cpp_properties.json @@ -0,0 +1,511 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/example_project/include", + "/home/fishros/chapt9/example_project/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/example_project/include", + "/home/fishros/chapt9/example_project/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60116", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/example_project/.vscode/extensions.json b/chapt9/example_project/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/example_project/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/example_project/.vscode/launch.json b/chapt9/example_project/.vscode/launch.json new file mode 100644 index 0000000..51d1516 --- /dev/null +++ b/chapt9/example_project/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/example_project/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/example_project/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/example_project/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/example_project/include/README b/chapt9/example_project/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/example_project/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/example_project/lib/README b/chapt9/example_project/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/example_project/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/example_project/platformio.ini b/chapt9/example_project/platformio.ini new file mode 100644 index 0000000..acc0f71 --- /dev/null +++ b/chapt9/example_project/platformio.ini @@ -0,0 +1,13 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino \ No newline at end of file diff --git a/chapt9/example_project/src/main.cpp b/chapt9/example_project/src/main.cpp new file mode 100644 index 0000000..634317c --- /dev/null +++ b/chapt9/example_project/src/main.cpp @@ -0,0 +1,13 @@ +#include +// setup 函数，启动时调用一次 +void setup() +{ + Serial.begin(115200); // 设置串口波特率 +} + +// loop 函数，setup 后会被重复调用 +void loop() +{ + Serial.printf("Hello World!\n"); // 打印Hello World! + delay(100); // 延时函数，单位 ms +} \ No newline at end of file diff --git a/chapt9/example_project/test/README b/chapt9/example_project/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/example_project/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/example_ultrasound/.gitignore b/chapt9/example_ultrasound/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/example_ultrasound/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/example_ultrasound/.vscode/c_cpp_properties.json b/chapt9/example_ultrasound/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..738b243 --- /dev/null +++ b/chapt9/example_ultrasound/.vscode/c_cpp_properties.json @@ -0,0 +1,513 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/example_ultrasound/include", + "/home/fishros/chapt9/example_ultrasound/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/chapt9/example_ultrasound/lib/PidController", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/example_ultrasound/include", + "/home/fishros/chapt9/example_ultrasound/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/chapt9/example_ultrasound/lib/PidController", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60116", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/example_ultrasound/.vscode/extensions.json b/chapt9/example_ultrasound/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/example_ultrasound/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/example_ultrasound/.vscode/launch.json b/chapt9/example_ultrasound/.vscode/launch.json new file mode 100644 index 0000000..8cdf043 --- /dev/null +++ b/chapt9/example_ultrasound/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/example_ultrasound/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/example_ultrasound/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/example_ultrasound/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/example_ultrasound/include/README b/chapt9/example_ultrasound/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/example_ultrasound/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/example_ultrasound/lib/PidController/PidController.cpp b/chapt9/example_ultrasound/lib/PidController/PidController.cpp new file mode 100644 index 0000000..e69de29 diff --git a/chapt9/example_ultrasound/lib/PidController/PidController.h b/chapt9/example_ultrasound/lib/PidController/PidController.h new file mode 100644 index 0000000..2329fee --- /dev/null +++ b/chapt9/example_ultrasound/lib/PidController/PidController.h @@ -0,0 +1,30 @@ +#ifndef __PID_CONTROLLER_H__ +#define __PID_CONTROLLER_H__ + + +class PidController +{ + public: + PidController() = default; + PidController(float kp, float ki, float kd, float dt); + + private: + float target_; + float kp_; + float ki_; + float kd_; + // pid + float error_; + float error_sum_; + float derror_; + float prev_error_; + float intergral_up_ = 2500; // 积分上限 + + public: + float update(float current); // 提供当前值，返回下次输出值，也就是PID的结果 + + +} + + +#endif // __PID_CONTROLLER_H__ \ No newline at end of file diff --git a/chapt9/example_ultrasound/lib/README b/chapt9/example_ultrasound/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/example_ultrasound/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/example_ultrasound/platformio.ini b/chapt9/example_ultrasound/platformio.ini new file mode 100644 index 0000000..acc0f71 --- /dev/null +++ b/chapt9/example_ultrasound/platformio.ini @@ -0,0 +1,13 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino \ No newline at end of file diff --git a/chapt9/example_ultrasound/src/main.cpp b/chapt9/example_ultrasound/src/main.cpp new file mode 100644 index 0000000..4fef637 --- /dev/null +++ b/chapt9/example_ultrasound/src/main.cpp @@ -0,0 +1,24 @@ +#include + +#define TRIG 27 +#define ECHO 21 + +void setup() +{ + Serial.begin(115200); // 初始化串口 + pinMode(2, OUTPUT); // 设置2号引脚模式为OUTPUT模式 + pinMode(TRIG, OUTPUT); // 设置TRIG引脚为输出模式 + pinMode(ECHO, INPUT); // 设置ECHO引脚为输入模式 +} +void loop() +{ + // 啊一下，产生一个10微秒的高电平 + digitalWrite(TRIG, HIGH); + delayMicroseconds(10); // 延时 10 微秒 + digitalWrite(TRIG, LOW); + // 读取ECHO引脚的高电平持续时间 + double delta_time = pulseIn(ECHO, HIGH); // 检测高电平持续时间，注意返回值是微秒us + float detect_distance = delta_time * 0.0343 / 2; // 计算距离单位 cm, 声速 0.0343 + Serial.printf("distance=%f cm\n", detect_distance); // 打印距离 + delay(500); // 延时 1 秒 +} \ No newline at end of file diff --git a/chapt9/example_ultrasound/test/README b/chapt9/example_ultrasound/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/example_ultrasound/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/fishbot_motion_control/.gitignore b/chapt9/fishbot_motion_control/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/fishbot_motion_control/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/fishbot_motion_control/.vscode/c_cpp_properties.json b/chapt9/fishbot_motion_control/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..d2e8835 --- /dev/null +++ b/chapt9/fishbot_motion_control/.vscode/c_cpp_properties.json @@ -0,0 +1,531 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/fishbot_motion_control/include", + "/home/fishros/chapt9/fishbot_motion_control/src", + "/home/fishros/chapt9/fishbot_motion_control/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/libmicroros/include", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino/wifi", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/fishbot_motion_control/include", + "/home/fishros/chapt9/fishbot_motion_control/src", + "/home/fishros/chapt9/fishbot_motion_control/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/libmicroros/include", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino/wifi", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60117", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "MICRO_ROS_TRANSPORT_ARDUINO_WIFI=1", + "MICRO_ROS_DISTRO_HUMBLE =1", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/fishbot_motion_control/.vscode/extensions.json b/chapt9/fishbot_motion_control/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/fishbot_motion_control/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/fishbot_motion_control/.vscode/launch.json b/chapt9/fishbot_motion_control/.vscode/launch.json new file mode 100644 index 0000000..30e099f --- /dev/null +++ b/chapt9/fishbot_motion_control/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/fishbot_motion_control/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/fishbot_motion_control/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/fishbot_motion_control/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/fishbot_motion_control/.vscode/settings.json b/chapt9/fishbot_motion_control/.vscode/settings.json new file mode 100644 index 0000000..1be854f --- /dev/null +++ b/chapt9/fishbot_motion_control/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "cmath": "cpp" + } +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control/README.md b/chapt9/fishbot_motion_control/README.md new file mode 100644 index 0000000..270da5d --- /dev/null +++ b/chapt9/fishbot_motion_control/README.md @@ -0,0 +1,4 @@ +一圈 1930 个脉冲数 +轮子的直径是 65mm +轮子转一圈是 65*PI +那么一个脉冲数，相当于走了多远距离 65*PI/1930 0.105805 mm \ No newline at end of file diff --git a/chapt9/fishbot_motion_control/include/README b/chapt9/fishbot_motion_control/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/fishbot_motion_control/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.cpp b/chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.cpp new file mode 100644 index 0000000..3deae23 --- /dev/null +++ b/chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.cpp @@ -0,0 +1,89 @@ +#include "Kinematics.h" + +// 设置轮子的间距 +void Kinematics::set_wheel_distance(float distance) +{ + wheel_distance = distance; +} + +void Kinematics::set_motor_param(uint8_t id, float per_pluse_distance) +{ + motor_param[id].per_pulse_distance = per_pluse_distance; +} + + +// 运动学正解,将左右轮的速度转换成线速度和角速度 +void Kinematics::kinematics_forward(float left_speed, float right_speed, float *out_linear_speed, float *out_angular_speed) +{ + *out_linear_speed = (left_speed + right_speed) / 2; + *out_angular_speed = (right_speed - left_speed) / wheel_distance; +} + + +// 运动学逆解，将线速度和角速度转换成左右轮的速度 +void Kinematics::kinematics_inverse(float linear_speed, float angular_speed, float *out_left_speed, float *out_right_speed) +{ + *out_left_speed = linear_speed - angular_speed * wheel_distance / 2; + *out_right_speed = linear_speed + angular_speed * wheel_distance / 2; +} + +// 输入：左右轮脉冲数，当前时间，输出更新电机速度和编码器数据 +void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick) +{ + int16_t dt = current_time - last_update_time; // ms + delta_ticks[0] = left_tick - motor_param[0].last_encoder_ticks; + delta_ticks[1] = right_tick - motor_param[1].last_encoder_ticks; + + motor_param[0].motor_speed = (delta_ticks[0] * 105.805) / dt; + motor_param[1].motor_speed = (delta_ticks[1] * 105.805) / dt; + + // 为了下次还可以正常的计算速度 + motor_param[0].last_encoder_ticks = left_tick; + motor_param[1].last_encoder_ticks = right_tick; + last_update_time = current_time; + + update_odom(dt); +} +// 获取电机速度,返回值是速度 +int16_t Kinematics::get_motor_speed(uint8_t id) +{ + if (id < 0 || id > 1) + { + return -1; + } + return motor_param[id].motor_speed; +} + + odom_t& Kinematics::get_odom() + { + return odom; + } + +void Kinematics::TransAngleInPI(float angle,float& out_angle) +{ + if(angle>PI) + { + out_angle -= 2*PI; + }else if (angle<-PI) + { + out_angle += 2*PI; + } +} + +void Kinematics::update_odom(uint16_t dt) +{ + float dt_s = float(dt)/1000.0; // ms -> s + // 获取实时的角速度和线速度呢？我们拿左右轮实时的速度，进行运动学正解 + this->kinematics_forward(motor_param[0].motor_speed,motor_param[1].motor_speed,&odom.linear_speed,&odom.angular_speed); + // 计算里程计信息 + odom.linear_speed = odom.linear_speed/1000.0; // 转换成米每秒 + + // 角度积分 + odom.angle += odom.angular_speed*dt_s; + TransAngleInPI(odom.angle,odom.angle); + // 计算机器人行走的距离（沿自身前进方向的） + float delta_distance = odom.linear_speed * dt_s; + // 分解到X轴和Y轴 + odom.x += delta_distance * std::cos(odom.angle); + odom.y += delta_distance * std::sin(odom.angle); +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.h b/chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.h new file mode 100644 index 0000000..6bc6db6 --- /dev/null +++ b/chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.h @@ -0,0 +1,57 @@ +#ifndef __KINEMATICS_H__ +#define __KINEMATICS_H__ + +#include "Arduino.h" + +typedef struct +{ + float per_pulse_distance; // 每个脉冲的前进的距离 + int16_t motor_speed; // 单位用mm/s + int64_t last_encoder_ticks; // 上一次点击的编码器读数 +} motor_param_t; + + +typedef struct { + float x; + float y; + float angle; + float linear_speed; + float angular_speed; +}odom_t; + +/** + * 1. 运动学正逆解（两个轮子的实时速度->当前实时的角速度和线速度 / 当前目标的角速度和线速度->两个轮子的目标速度） + * + */ +class Kinematics +{ +private: + /* data */ + motor_param_t motor_param[2]; + int16_t delta_ticks[2] = {0, 0}; // 用于存储上一次读取的编码器数值 + uint64_t last_update_time = 0; // 用于存储上一次更新电机速度的时间,计算速度的时候使用 + float wheel_distance = 0.0; // 两个轮子之间的距离 + odom_t odom; // 用于存储里程计信息 +public: + Kinematics(/* args */) = default; + ~Kinematics() = default; + + odom_t& get_odom(); + void update_odom(uint16_t dt); + void TransAngleInPI(float angle,float& out_angle); + + void set_wheel_distance(float distance); // 设置轮子的间距 + + void set_motor_param(uint8_t id,float per_pluse_distance); + // 运动学正解,将左右轮的速度转换成线速度和角速度 + void kinematics_forward(float left_speed,float right_speed,float* out_linear_speed,float* out_angular_speed); + // 运动学逆解，将线速度和角速度转换成左右轮的速度 + void kinematics_inverse(float linear_speed,float angular_speed,float* out_left_speed,float* out_right_speed); + // 更新点击速度和编码器数据 + void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick); + // 获取电机速度,返回值是速度 + int16_t get_motor_speed(uint8_t id); + +}; + +#endif // __KINEMATICS_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control/lib/PidController/PidController.cpp b/chapt9/fishbot_motion_control/lib/PidController/PidController.cpp new file mode 100644 index 0000000..c5b5d77 --- /dev/null +++ b/chapt9/fishbot_motion_control/lib/PidController/PidController.cpp @@ -0,0 +1,64 @@ +#include "Arduino.h" +#include "PidController.h" + +// 构造函数，传入三个PID参数 +PidController::PidController(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +float PidController::update(float current) +{ + error_ = target_ - current; // 计算error + + error_sum_ += error_; // 计算error_sum,同时限制积分上下限 + if (error_sum_ > intergral_up_) + error_sum_ = intergral_up_; + if (error_sum_ < -1 * intergral_up_) + error_sum_ = -1 * intergral_up_; + + derror_ = prev_error_ - error_; // 计算误差变化率 + prev_error_ = error_; // 方便下次计算使用 + + float output = kp_ * error_ + ki_ * error_sum_ + kd_ * derror_; + + if (output > out_max_) + output = out_max_; + if (output < out_min_) + output = out_min_; + + return output; +} + +void PidController::update_target(float target) +{ + target_ = target; +} + +void PidController::update_pid(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +void PidController::reset() +{ + error_sum_ = 0; + prev_error_ = 0; + error_ = 0; + derror_ = 0; + kp_ = 0; + ki_ = 0; + kd_ = 0; + intergral_up_ = 2500; + out_min_ = 0; + out_max_ = 0; +} +void PidController::out_limit(float min, float max) +{ + out_min_ = min; + out_max_ = max; +} diff --git a/chapt9/fishbot_motion_control/lib/PidController/PidController.h b/chapt9/fishbot_motion_control/lib/PidController/PidController.h new file mode 100644 index 0000000..d184c90 --- /dev/null +++ b/chapt9/fishbot_motion_control/lib/PidController/PidController.h @@ -0,0 +1,33 @@ +#ifndef __PID_CONTROLLER_H__ +#define __PID_CONTROLLER_H__ + +class PidController +{ +public: + PidController() = default; + PidController(float kp, float ki, float kd); + +private: + // PID 参数，可以调节的 + float target_; + float out_min_; + float out_max_; + float kp_; + float ki_; + float kd_; + float intergral_up_ = 2500; // 积分上限 + // pid 中间过程值 + float error_; + float error_sum_; + float derror_; + float prev_error_; + +public: + float update(float current); // 提供当前值，返回下次输出值，也就是PID的结果 + void update_target(float target); // 更新目标值 + void update_pid(float kp, float ki, float kd); // 更新PID参数 + void reset(); // 重置PID + void out_limit(float min, float max); // 设置输出限制 +}; + +#endif // __PID_CONTROLLER_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control/lib/README b/chapt9/fishbot_motion_control/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/fishbot_motion_control/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/fishbot_motion_control/platformio.ini b/chapt9/fishbot_motion_control/platformio.ini new file mode 100644 index 0000000..6915ea3 --- /dev/null +++ b/chapt9/fishbot_motion_control/platformio.ini @@ -0,0 +1,19 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino +board_microros_transport = wifi +monitor_speed = 115200 +lib_deps = + http://github.fishros.org/https://github.com/fishros/Esp32McpwmMotor.git + http://github.fishros.org/https://github.com/fishros/Esp32PcntEncoder.git + https://gitee.com/ohhuo/micro_ros_platformio.git \ No newline at end of file diff --git a/chapt9/fishbot_motion_control/src/main.cpp b/chapt9/fishbot_motion_control/src/main.cpp new file mode 100644 index 0000000..57e09d3 --- /dev/null +++ b/chapt9/fishbot_motion_control/src/main.cpp @@ -0,0 +1,152 @@ +#include +#include +#include +#include +#include + +// 引入Microros和wifi相关的库 +#include +#include +#include +#include +#include +#include // 消息接口 +#include // 里程计消息接口 +#include // 引入字符串内存分配初始化工具 + +// 声明一些相关的结构体对象 +rcl_allocator_t allocator; // 内存分配器，用于动态内存分配管理 +rclc_support_t support; // 用于存储时钟，内存分配器和上下文，用于提供支持 +rclc_executor_t executor; // 执行器，用于管理订阅和计时器回调的执行 +rcl_node_t node; // 节点，用于创建节点 +rcl_subscription_t sub_cmd_vel; // 创建一个订阅者 +geometry_msgs__msg__Twist msg_cmd_vel; // 订阅到的数据存储到这里 + +rcl_publisher_t pub_odom; // 创建一个里程计发布者 +nav_msgs__msg__Odometry msg_odom; // 里程计消息存储到这里 +rcl_timer_t timer; // 定时器，可以定时调用某个函数 + + + + +Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 +Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 +PidController pid_controller[2]; +Kinematics kinematics; + +float target_linear_speed = 0.0; // 单位 毫米每秒 +float target_angular_speed = 0.0; // 单位 弧度每秒 +float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 +float out_right_speed = 0.0; + +// 定时器的回调函数 +void timer_callback(rcl_timer_t* timer,int64_t last_call_time) +{ + // 完成里程计的发布 + odom_t odom = kinematics.get_odom(); // 获取当前的里程计 + int64_t stamp = rmw_uros_epoch_millis(); // 获取当前的时间 + msg_odom.header.stamp.sec = static_cast(stamp/1000); // 秒部分 + msg_odom.header.stamp.nanosec = static_cast((stamp%1000)*1e6); // 纳秒部分 + msg_odom.pose.pose.position.x = odom.x; + msg_odom.pose.pose.position.y = odom.y; + msg_odom.pose.pose.orientation.w = cos(odom.angle*0.5); + msg_odom.pose.pose.orientation.x = 0; + msg_odom.pose.pose.orientation.y = 0; + msg_odom.pose.pose.orientation.z = sin(odom.angle*0.5); + msg_odom.twist.twist.linear.x = odom.linear_speed; + msg_odom.twist.twist.angular.z = odom.angular_speed; + // 发布里程计，把数据发出去 + if(rcl_publish(&pub_odom,&msg_odom,NULL)!=RCL_RET_OK) + { + Serial.println("error: odom pub failed!"); + } +} + + +void twist_callback(const void * msg_in) +{ + // 将受到的消息指针转换成 geometry_msgs__msg__Twist 类型的指针 + const geometry_msgs__msg__Twist* msg = (const geometry_msgs__msg__Twist*)msg_in; + target_linear_speed = msg->linear.x * 1000; + target_angular_speed = msg->angular.z; + kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); + Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); + pid_controller[0].update_target(out_left_speed); + pid_controller[1].update_target(out_right_speed); +} + +// 单独创建一个任务运行 micro-ROS 相当于一个线程 +void microros_task(void *args) +{ + // 1.设置传输协议并延迟一段时间等待设置的完成 + IPAddress agent_ip; + agent_ip.fromString("192.168.1.103"); // 设置agent的IP地址 + set_microros_wifi_transports("fishros", "88888888", agent_ip, 8888); // 设置传输协议 + delay(3000); // 等待2秒,等待WIFI连接 + // 2.初始化内存分配器 + allocator = rcl_get_default_allocator(); // 获取默认的内存分配器 + // 3.初始化支持 + rclc_support_init(&support, 0, NULL, &allocator); // 初始化支持 + // 4.初始化节点 + rclc_node_init_default(&node, "fishbot_motion_control", "", &support); // 初始化节点 + // 5.初始化执行器 + unsigned int num_handles = 2; // 订阅和计时器的回调数量,注意这是一个要改的参数 + rclc_executor_init(&executor, &support.context, num_handles, &allocator); // 初始化执行器 + // 初始化订阅者，并将其添加到执行其中 + rclc_subscription_init_best_effort(&sub_cmd_vel,&node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Twist),"/cmd_vel"); + rclc_executor_add_subscription(&executor,&sub_cmd_vel,&msg_cmd_vel,&twist_callback,ON_NEW_DATA); + // 初始化msg + msg_odom.header.frame_id = micro_ros_string_utilities_set(msg_odom.header.frame_id, "odom"); + msg_odom.child_frame_id = micro_ros_string_utilities_set(msg_odom.child_frame_id, "base_footprint"); + // 初始化发布者和定时器 + rclc_publisher_init_best_effort(&pub_odom,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs,msg,Odometry),"/odom"); + rclc_timer_init_default(&timer,&support,RCL_MS_TO_NS(50),timer_callback); + rclc_executor_add_timer(&executor,&timer); + // 时间同步 + while (!rmw_uros_epoch_synchronized()) + { + rmw_uros_sync_session(1000); + delay(10); + } + // 循环执行器 + rclc_executor_spin(&executor); +} + +// v=w*r +// r = v/w = 0.05/0.1 = 0.5 0.02/0.1 = 0.2 m +void setup() +{ + // 初始化串口 + Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 + // 初始化电机驱动器 + motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23 + motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13 + // 初始化编码器 + encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 + encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 + // 初始化PID控制器的参数 + pid_controller[0].update_pid(0.625, 0.125, 0.0); + pid_controller[1].update_pid(0.625, 0.125, 0.0); + pid_controller[0].out_limit(-100, 100); + pid_controller[1].out_limit(-100, 100); + // 初始化运动学参数 + kinematics.set_wheel_distance(175); // mm + kinematics.set_motor_param(0, 0.105805); + kinematics.set_motor_param(1, 0.105805); + // 测试下运动学逆解 + + + // 创建一个任务运行 micro-ROS + xTaskCreate(microros_task, "microros_task", 10240, NULL, 1, NULL); +} + +void loop() +{ + delay(10); // 等待10毫秒 + kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); // 记得调用更新电机速度函数 + motor.updateMotorSpeed(0, pid_controller[0].update( + kinematics.get_motor_speed(0))); + motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1))); + // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); + // Serial.printf("x,y,yaw=%f,%f,%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); +} diff --git a/chapt9/fishbot_motion_control/test/9.4.1.cpp b/chapt9/fishbot_motion_control/test/9.4.1.cpp new file mode 100644 index 0000000..283ef1e --- /dev/null +++ b/chapt9/fishbot_motion_control/test/9.4.1.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include +#include + +// 引入Microros和wifi相关的库 +#include +#include +#include +#include +#include + +// 声明一些相关的结构体对象 +rcl_allocator_t allocator; // 内存分配器，用于动态内存分配管理 +rclc_support_t support; // 用于存储时钟，内存分配器和上下文，用于提供支持 +rclc_executor_t executor; // 执行器，用于管理订阅和计时器回调的执行 +rcl_node_t node; // 节点，用于创建节点 + +// 单独创建一个任务运行 micro-ROS 相当于一个线程 +void microros_task(void *args) +{ + // 1.设置传输协议并延迟一段时间等待设置的完成 + IPAddress agent_ip; + agent_ip.fromString("192.168.1.103"); // 设置agent的IP地址 + set_microros_wifi_transports("fishros", "88888888", agent_ip, 8888); // 设置传输协议 + delay(3000); // 等待2秒,等待WIFI连接 + // 2.初始化内存分配器 + allocator = rcl_get_default_allocator(); // 获取默认的内存分配器 + // 3.初始化支持 + rclc_support_init(&support, 0, NULL, &allocator); // 初始化支持 + // 4.初始化节点 + rclc_node_init_default(&node, "fishbot_motion_control", "", &support); // 初始化节点 + // 5.初始化执行器 + unsigned int num_handles = 1; // 订阅和计时器的回调数量,注意这是一个要改的参数 + rclc_executor_init(&executor, &support.context, num_handles, &allocator); // 初始化执行器 + // 循环执行器 + rclc_executor_spin(&executor); +} + +Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 +Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 +PidController pid_controller[2]; +Kinematics kinematics; + +float target_linear_speed = 0.0; // 单位 毫米每秒 +float target_angular_speed = 0.0; // 单位 弧度每秒 +float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 +float out_right_speed = 0.0; + +// v=w*r +// r = v/w = 0.05/0.1 = 0.5 0.02/0.1 = 0.2 m +void setup() +{ + // 初始化串口 + Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 + // 初始化电机驱动器 + motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23 + motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13 + // 初始化编码器 + encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 + encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 + // 初始化PID控制器的参数 + pid_controller[0].update_pid(0.625, 0.125, 0.0); + pid_controller[1].update_pid(0.625, 0.125, 0.0); + pid_controller[0].out_limit(-100, 100); + pid_controller[1].out_limit(-100, 100); + // 初始化运动学参数 + kinematics.set_wheel_distance(175); // mm + kinematics.set_motor_param(0, 0.105805); + kinematics.set_motor_param(1, 0.105805); + // 测试下运动学逆解 + kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); + Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); + pid_controller[0].update_target(out_left_speed); + pid_controller[1].update_target(out_right_speed); + + // 创建一个任务运行 micro-ROS + xTaskCreate(microros_task, "microros_task", 10240, NULL, 1, NULL); +} + +void loop() +{ + delay(10); // 等待10毫秒 + kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); // 记得调用更新电机速度函数 + motor.updateMotorSpeed(0, pid_controller[0].update( + kinematics.get_motor_speed(0))); + motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1))); + // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); + // Serial.printf("x,y,yaw=%f,%f,%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); +} diff --git a/chapt9/fishbot_motion_control/test/9.4.2.cpp b/chapt9/fishbot_motion_control/test/9.4.2.cpp new file mode 100644 index 0000000..99a72b3 --- /dev/null +++ b/chapt9/fishbot_motion_control/test/9.4.2.cpp @@ -0,0 +1,108 @@ +#include +#include +#include +#include +#include + +// 引入Microros和wifi相关的库 +#include +#include +#include +#include +#include +#include // 消息接口 + +// 声明一些相关的结构体对象 +rcl_allocator_t allocator; // 内存分配器，用于动态内存分配管理 +rclc_support_t support; // 用于存储时钟，内存分配器和上下文，用于提供支持 +rclc_executor_t executor; // 执行器，用于管理订阅和计时器回调的执行 +rcl_node_t node; // 节点，用于创建节点 +rcl_subscription_t sub_cmd_vel; // 创建一个订阅者 +geometry_msgs__msg__Twist msg_cmd_vel; // 订阅到的数据存储到这里 + + +Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 +Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 +PidController pid_controller[2]; +Kinematics kinematics; + +float target_linear_speed = 0.0; // 单位 毫米每秒 +float target_angular_speed = 0.0; // 单位 弧度每秒 +float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 +float out_right_speed = 0.0; + + +void twist_callback(const void * msg_in) +{ + // 将受到的消息指针转换成 geometry_msgs__msg__Twist 类型的指针 + const geometry_msgs__msg__Twist* msg = (const geometry_msgs__msg__Twist*)msg_in; + target_linear_speed = msg->linear.x * 1000; + target_angular_speed = msg->angular.z; + kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); + Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); + pid_controller[0].update_target(out_left_speed); + pid_controller[1].update_target(out_right_speed); +} + +// 单独创建一个任务运行 micro-ROS 相当于一个线程 +void microros_task(void *args) +{ + // 1.设置传输协议并延迟一段时间等待设置的完成 + IPAddress agent_ip; + agent_ip.fromString("192.168.1.103"); // 设置agent的IP地址 + set_microros_wifi_transports("fishros", "88888888", agent_ip, 8888); // 设置传输协议 + delay(3000); // 等待2秒,等待WIFI连接 + // 2.初始化内存分配器 + allocator = rcl_get_default_allocator(); // 获取默认的内存分配器 + // 3.初始化支持 + rclc_support_init(&support, 0, NULL, &allocator); // 初始化支持 + // 4.初始化节点 + rclc_node_init_default(&node, "fishbot_motion_control", "", &support); // 初始化节点 + // 5.初始化执行器 + unsigned int num_handles = 1; // 订阅和计时器的回调数量,注意这是一个要改的参数 + rclc_executor_init(&executor, &support.context, num_handles, &allocator); // 初始化执行器 + // 初始化订阅者，并将其添加到执行其中 + rclc_subscription_init_best_effort(&sub_cmd_vel,&node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Twist),"/cmd_vel"); + rclc_executor_add_subscription(&executor,&sub_cmd_vel,&msg_cmd_vel,&twist_callback,ON_NEW_DATA); + // 循环执行器 + rclc_executor_spin(&executor); +} + +// v=w*r +// r = v/w = 0.05/0.1 = 0.5 0.02/0.1 = 0.2 m +void setup() +{ + // 初始化串口 + Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 + // 初始化电机驱动器 + motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23 + motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13 + // 初始化编码器 + encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 + encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 + // 初始化PID控制器的参数 + pid_controller[0].update_pid(0.625, 0.125, 0.0); + pid_controller[1].update_pid(0.625, 0.125, 0.0); + pid_controller[0].out_limit(-100, 100); + pid_controller[1].out_limit(-100, 100); + // 初始化运动学参数 + kinematics.set_wheel_distance(175); // mm + kinematics.set_motor_param(0, 0.105805); + kinematics.set_motor_param(1, 0.105805); + // 测试下运动学逆解 + + + // 创建一个任务运行 micro-ROS + xTaskCreate(microros_task, "microros_task", 10240, NULL, 1, NULL); +} + +void loop() +{ + delay(10); // 等待10毫秒 + kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); // 记得调用更新电机速度函数 + motor.updateMotorSpeed(0, pid_controller[0].update( + kinematics.get_motor_speed(0))); + motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1))); + // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); + // Serial.printf("x,y,yaw=%f,%f,%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); +} diff --git a/chapt9/fishbot_motion_control/test/9.4.3.cpp b/chapt9/fishbot_motion_control/test/9.4.3.cpp new file mode 100644 index 0000000..283ef1e --- /dev/null +++ b/chapt9/fishbot_motion_control/test/9.4.3.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include +#include + +// 引入Microros和wifi相关的库 +#include +#include +#include +#include +#include + +// 声明一些相关的结构体对象 +rcl_allocator_t allocator; // 内存分配器，用于动态内存分配管理 +rclc_support_t support; // 用于存储时钟，内存分配器和上下文，用于提供支持 +rclc_executor_t executor; // 执行器，用于管理订阅和计时器回调的执行 +rcl_node_t node; // 节点，用于创建节点 + +// 单独创建一个任务运行 micro-ROS 相当于一个线程 +void microros_task(void *args) +{ + // 1.设置传输协议并延迟一段时间等待设置的完成 + IPAddress agent_ip; + agent_ip.fromString("192.168.1.103"); // 设置agent的IP地址 + set_microros_wifi_transports("fishros", "88888888", agent_ip, 8888); // 设置传输协议 + delay(3000); // 等待2秒,等待WIFI连接 + // 2.初始化内存分配器 + allocator = rcl_get_default_allocator(); // 获取默认的内存分配器 + // 3.初始化支持 + rclc_support_init(&support, 0, NULL, &allocator); // 初始化支持 + // 4.初始化节点 + rclc_node_init_default(&node, "fishbot_motion_control", "", &support); // 初始化节点 + // 5.初始化执行器 + unsigned int num_handles = 1; // 订阅和计时器的回调数量,注意这是一个要改的参数 + rclc_executor_init(&executor, &support.context, num_handles, &allocator); // 初始化执行器 + // 循环执行器 + rclc_executor_spin(&executor); +} + +Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 +Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 +PidController pid_controller[2]; +Kinematics kinematics; + +float target_linear_speed = 0.0; // 单位 毫米每秒 +float target_angular_speed = 0.0; // 单位 弧度每秒 +float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 +float out_right_speed = 0.0; + +// v=w*r +// r = v/w = 0.05/0.1 = 0.5 0.02/0.1 = 0.2 m +void setup() +{ + // 初始化串口 + Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 + // 初始化电机驱动器 + motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23 + motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13 + // 初始化编码器 + encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 + encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 + // 初始化PID控制器的参数 + pid_controller[0].update_pid(0.625, 0.125, 0.0); + pid_controller[1].update_pid(0.625, 0.125, 0.0); + pid_controller[0].out_limit(-100, 100); + pid_controller[1].out_limit(-100, 100); + // 初始化运动学参数 + kinematics.set_wheel_distance(175); // mm + kinematics.set_motor_param(0, 0.105805); + kinematics.set_motor_param(1, 0.105805); + // 测试下运动学逆解 + kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); + Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); + pid_controller[0].update_target(out_left_speed); + pid_controller[1].update_target(out_right_speed); + + // 创建一个任务运行 micro-ROS + xTaskCreate(microros_task, "microros_task", 10240, NULL, 1, NULL); +} + +void loop() +{ + delay(10); // 等待10毫秒 + kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); // 记得调用更新电机速度函数 + motor.updateMotorSpeed(0, pid_controller[0].update( + kinematics.get_motor_speed(0))); + motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1))); + // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); + // Serial.printf("x,y,yaw=%f,%f,%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); +} diff --git a/chapt9/fishbot_motion_control/test/README b/chapt9/fishbot_motion_control/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/fishbot_motion_control/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/fishbot_motion_control_9.3.4/.gitignore b/chapt9/fishbot_motion_control_9.3.4/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/fishbot_motion_control_9.3.4/.vscode/c_cpp_properties.json b/chapt9/fishbot_motion_control_9.3.4/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..033c1c9 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/.vscode/c_cpp_properties.json @@ -0,0 +1,519 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/include", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/src", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/include", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/src", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control_9.3.4/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60116", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/fishbot_motion_control_9.3.4/.vscode/extensions.json b/chapt9/fishbot_motion_control_9.3.4/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/fishbot_motion_control_9.3.4/.vscode/launch.json b/chapt9/fishbot_motion_control_9.3.4/.vscode/launch.json new file mode 100644 index 0000000..f6d9029 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/fishbot_motion_control_9.3.4/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/fishbot_motion_control_9.3.4/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/fishbot_motion_control_9.3.4/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/fishbot_motion_control_9.3.4/.vscode/settings.json b/chapt9/fishbot_motion_control_9.3.4/.vscode/settings.json new file mode 100644 index 0000000..1be854f --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "cmath": "cpp" + } +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3.4/README.md b/chapt9/fishbot_motion_control_9.3.4/README.md new file mode 100644 index 0000000..270da5d --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/README.md @@ -0,0 +1,4 @@ +一圈 1930 个脉冲数 +轮子的直径是 65mm +轮子转一圈是 65*PI +那么一个脉冲数，相当于走了多远距离 65*PI/1930 0.105805 mm \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3.4/include/README b/chapt9/fishbot_motion_control_9.3.4/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.cpp b/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.cpp new file mode 100644 index 0000000..3deae23 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.cpp @@ -0,0 +1,89 @@ +#include "Kinematics.h" + +// 设置轮子的间距 +void Kinematics::set_wheel_distance(float distance) +{ + wheel_distance = distance; +} + +void Kinematics::set_motor_param(uint8_t id, float per_pluse_distance) +{ + motor_param[id].per_pulse_distance = per_pluse_distance; +} + + +// 运动学正解,将左右轮的速度转换成线速度和角速度 +void Kinematics::kinematics_forward(float left_speed, float right_speed, float *out_linear_speed, float *out_angular_speed) +{ + *out_linear_speed = (left_speed + right_speed) / 2; + *out_angular_speed = (right_speed - left_speed) / wheel_distance; +} + + +// 运动学逆解，将线速度和角速度转换成左右轮的速度 +void Kinematics::kinematics_inverse(float linear_speed, float angular_speed, float *out_left_speed, float *out_right_speed) +{ + *out_left_speed = linear_speed - angular_speed * wheel_distance / 2; + *out_right_speed = linear_speed + angular_speed * wheel_distance / 2; +} + +// 输入：左右轮脉冲数，当前时间，输出更新电机速度和编码器数据 +void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick) +{ + int16_t dt = current_time - last_update_time; // ms + delta_ticks[0] = left_tick - motor_param[0].last_encoder_ticks; + delta_ticks[1] = right_tick - motor_param[1].last_encoder_ticks; + + motor_param[0].motor_speed = (delta_ticks[0] * 105.805) / dt; + motor_param[1].motor_speed = (delta_ticks[1] * 105.805) / dt; + + // 为了下次还可以正常的计算速度 + motor_param[0].last_encoder_ticks = left_tick; + motor_param[1].last_encoder_ticks = right_tick; + last_update_time = current_time; + + update_odom(dt); +} +// 获取电机速度,返回值是速度 +int16_t Kinematics::get_motor_speed(uint8_t id) +{ + if (id < 0 || id > 1) + { + return -1; + } + return motor_param[id].motor_speed; +} + + odom_t& Kinematics::get_odom() + { + return odom; + } + +void Kinematics::TransAngleInPI(float angle,float& out_angle) +{ + if(angle>PI) + { + out_angle -= 2*PI; + }else if (angle<-PI) + { + out_angle += 2*PI; + } +} + +void Kinematics::update_odom(uint16_t dt) +{ + float dt_s = float(dt)/1000.0; // ms -> s + // 获取实时的角速度和线速度呢？我们拿左右轮实时的速度，进行运动学正解 + this->kinematics_forward(motor_param[0].motor_speed,motor_param[1].motor_speed,&odom.linear_speed,&odom.angular_speed); + // 计算里程计信息 + odom.linear_speed = odom.linear_speed/1000.0; // 转换成米每秒 + + // 角度积分 + odom.angle += odom.angular_speed*dt_s; + TransAngleInPI(odom.angle,odom.angle); + // 计算机器人行走的距离（沿自身前进方向的） + float delta_distance = odom.linear_speed * dt_s; + // 分解到X轴和Y轴 + odom.x += delta_distance * std::cos(odom.angle); + odom.y += delta_distance * std::sin(odom.angle); +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.h b/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.h new file mode 100644 index 0000000..6bc6db6 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/lib/Kinematics/Kinematics.h @@ -0,0 +1,57 @@ +#ifndef __KINEMATICS_H__ +#define __KINEMATICS_H__ + +#include "Arduino.h" + +typedef struct +{ + float per_pulse_distance; // 每个脉冲的前进的距离 + int16_t motor_speed; // 单位用mm/s + int64_t last_encoder_ticks; // 上一次点击的编码器读数 +} motor_param_t; + + +typedef struct { + float x; + float y; + float angle; + float linear_speed; + float angular_speed; +}odom_t; + +/** + * 1. 运动学正逆解（两个轮子的实时速度->当前实时的角速度和线速度 / 当前目标的角速度和线速度->两个轮子的目标速度） + * + */ +class Kinematics +{ +private: + /* data */ + motor_param_t motor_param[2]; + int16_t delta_ticks[2] = {0, 0}; // 用于存储上一次读取的编码器数值 + uint64_t last_update_time = 0; // 用于存储上一次更新电机速度的时间,计算速度的时候使用 + float wheel_distance = 0.0; // 两个轮子之间的距离 + odom_t odom; // 用于存储里程计信息 +public: + Kinematics(/* args */) = default; + ~Kinematics() = default; + + odom_t& get_odom(); + void update_odom(uint16_t dt); + void TransAngleInPI(float angle,float& out_angle); + + void set_wheel_distance(float distance); // 设置轮子的间距 + + void set_motor_param(uint8_t id,float per_pluse_distance); + // 运动学正解,将左右轮的速度转换成线速度和角速度 + void kinematics_forward(float left_speed,float right_speed,float* out_linear_speed,float* out_angular_speed); + // 运动学逆解，将线速度和角速度转换成左右轮的速度 + void kinematics_inverse(float linear_speed,float angular_speed,float* out_left_speed,float* out_right_speed); + // 更新点击速度和编码器数据 + void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick); + // 获取电机速度,返回值是速度 + int16_t get_motor_speed(uint8_t id); + +}; + +#endif // __KINEMATICS_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.cpp b/chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.cpp new file mode 100644 index 0000000..c5b5d77 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.cpp @@ -0,0 +1,64 @@ +#include "Arduino.h" +#include "PidController.h" + +// 构造函数，传入三个PID参数 +PidController::PidController(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +float PidController::update(float current) +{ + error_ = target_ - current; // 计算error + + error_sum_ += error_; // 计算error_sum,同时限制积分上下限 + if (error_sum_ > intergral_up_) + error_sum_ = intergral_up_; + if (error_sum_ < -1 * intergral_up_) + error_sum_ = -1 * intergral_up_; + + derror_ = prev_error_ - error_; // 计算误差变化率 + prev_error_ = error_; // 方便下次计算使用 + + float output = kp_ * error_ + ki_ * error_sum_ + kd_ * derror_; + + if (output > out_max_) + output = out_max_; + if (output < out_min_) + output = out_min_; + + return output; +} + +void PidController::update_target(float target) +{ + target_ = target; +} + +void PidController::update_pid(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +void PidController::reset() +{ + error_sum_ = 0; + prev_error_ = 0; + error_ = 0; + derror_ = 0; + kp_ = 0; + ki_ = 0; + kd_ = 0; + intergral_up_ = 2500; + out_min_ = 0; + out_max_ = 0; +} +void PidController::out_limit(float min, float max) +{ + out_min_ = min; + out_max_ = max; +} diff --git a/chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.h b/chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.h new file mode 100644 index 0000000..d184c90 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/lib/PidController/PidController.h @@ -0,0 +1,33 @@ +#ifndef __PID_CONTROLLER_H__ +#define __PID_CONTROLLER_H__ + +class PidController +{ +public: + PidController() = default; + PidController(float kp, float ki, float kd); + +private: + // PID 参数，可以调节的 + float target_; + float out_min_; + float out_max_; + float kp_; + float ki_; + float kd_; + float intergral_up_ = 2500; // 积分上限 + // pid 中间过程值 + float error_; + float error_sum_; + float derror_; + float prev_error_; + +public: + float update(float current); // 提供当前值，返回下次输出值，也就是PID的结果 + void update_target(float target); // 更新目标值 + void update_pid(float kp, float ki, float kd); // 更新PID参数 + void reset(); // 重置PID + void out_limit(float min, float max); // 设置输出限制 +}; + +#endif // __PID_CONTROLLER_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3.4/lib/README b/chapt9/fishbot_motion_control_9.3.4/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/fishbot_motion_control_9.3.4/platformio.ini b/chapt9/fishbot_motion_control_9.3.4/platformio.ini new file mode 100644 index 0000000..8dfac39 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/platformio.ini @@ -0,0 +1,16 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino +lib_deps = + http://github.fishros.org/https://github.com/fishros/Esp32McpwmMotor.git + http://github.fishros.org/https://github.com/fishros/Esp32PcntEncoder.git \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3.4/src/main.cpp b/chapt9/fishbot_motion_control_9.3.4/src/main.cpp new file mode 100644 index 0000000..a26e1fe --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/src/main.cpp @@ -0,0 +1,52 @@ +#include +#include +#include +#include +#include +Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 +Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 +PidController pid_controller[2]; +Kinematics kinematics; + +float target_linear_speed = 20.0; // 单位 毫米每秒 +float target_angular_speed = 0.1; // 单位 弧度每秒 +float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 +float out_right_speed = 0.0; + +// v=wr r = v/w +void setup() +{ + // 初始化串口 + Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 + // 初始化电机驱动器 + motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23 + motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13 + // 初始化编码器 + encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 + encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 + // 初始化PID控制器的参数 + pid_controller[0].update_pid(0.625, 0.125, 0.0); + pid_controller[1].update_pid(0.625, 0.125, 0.0); + pid_controller[0].out_limit(-100, 100); + pid_controller[1].out_limit(-100, 100); + // 初始化运动学参数 + kinematics.set_wheel_distance(175); // mm + kinematics.set_motor_param(0, 0.105805); + kinematics.set_motor_param(1, 0.105805); + // 测试下运动学逆解 + kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); + Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); + pid_controller[0].update_target(out_left_speed); + pid_controller[1].update_target(out_right_speed); +} + +void loop() +{ + delay(10); // 等待10毫秒 + // 调用PID，获取动态输出值 + kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); + motor.updateMotorSpeed(0, pid_controller[0].update( + kinematics.get_motor_speed(0))); + motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1))); + // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); +} diff --git a/chapt9/fishbot_motion_control_9.3.4/test/README b/chapt9/fishbot_motion_control_9.3.4/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3.4/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/fishbot_motion_control_9.3/.gitignore b/chapt9/fishbot_motion_control_9.3/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/fishbot_motion_control_9.3/.vscode/c_cpp_properties.json b/chapt9/fishbot_motion_control_9.3/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..dc8c913 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/.vscode/c_cpp_properties.json @@ -0,0 +1,519 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/fishbot_motion_control_9.3/include", + "/home/fishros/chapt9/fishbot_motion_control_9.3/src", + "/home/fishros/chapt9/fishbot_motion_control_9.3/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control_9.3/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control_9.3/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control_9.3/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/fishbot_motion_control_9.3/include", + "/home/fishros/chapt9/fishbot_motion_control_9.3/src", + "/home/fishros/chapt9/fishbot_motion_control_9.3/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control_9.3/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control_9.3/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control_9.3/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60116", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/fishbot_motion_control_9.3/.vscode/extensions.json b/chapt9/fishbot_motion_control_9.3/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/fishbot_motion_control_9.3/.vscode/launch.json b/chapt9/fishbot_motion_control_9.3/.vscode/launch.json new file mode 100644 index 0000000..37341da --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/fishbot_motion_control_9.3/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/fishbot_motion_control_9.3/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/fishbot_motion_control_9.3/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/fishbot_motion_control_9.3/.vscode/settings.json b/chapt9/fishbot_motion_control_9.3/.vscode/settings.json new file mode 100644 index 0000000..1be854f --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "cmath": "cpp" + } +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3/README.md b/chapt9/fishbot_motion_control_9.3/README.md new file mode 100644 index 0000000..270da5d --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/README.md @@ -0,0 +1,4 @@ +一圈 1930 个脉冲数 +轮子的直径是 65mm +轮子转一圈是 65*PI +那么一个脉冲数，相当于走了多远距离 65*PI/1930 0.105805 mm \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3/include/README b/chapt9/fishbot_motion_control_9.3/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.cpp b/chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.cpp new file mode 100644 index 0000000..3deae23 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.cpp @@ -0,0 +1,89 @@ +#include "Kinematics.h" + +// 设置轮子的间距 +void Kinematics::set_wheel_distance(float distance) +{ + wheel_distance = distance; +} + +void Kinematics::set_motor_param(uint8_t id, float per_pluse_distance) +{ + motor_param[id].per_pulse_distance = per_pluse_distance; +} + + +// 运动学正解,将左右轮的速度转换成线速度和角速度 +void Kinematics::kinematics_forward(float left_speed, float right_speed, float *out_linear_speed, float *out_angular_speed) +{ + *out_linear_speed = (left_speed + right_speed) / 2; + *out_angular_speed = (right_speed - left_speed) / wheel_distance; +} + + +// 运动学逆解，将线速度和角速度转换成左右轮的速度 +void Kinematics::kinematics_inverse(float linear_speed, float angular_speed, float *out_left_speed, float *out_right_speed) +{ + *out_left_speed = linear_speed - angular_speed * wheel_distance / 2; + *out_right_speed = linear_speed + angular_speed * wheel_distance / 2; +} + +// 输入：左右轮脉冲数，当前时间，输出更新电机速度和编码器数据 +void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick) +{ + int16_t dt = current_time - last_update_time; // ms + delta_ticks[0] = left_tick - motor_param[0].last_encoder_ticks; + delta_ticks[1] = right_tick - motor_param[1].last_encoder_ticks; + + motor_param[0].motor_speed = (delta_ticks[0] * 105.805) / dt; + motor_param[1].motor_speed = (delta_ticks[1] * 105.805) / dt; + + // 为了下次还可以正常的计算速度 + motor_param[0].last_encoder_ticks = left_tick; + motor_param[1].last_encoder_ticks = right_tick; + last_update_time = current_time; + + update_odom(dt); +} +// 获取电机速度,返回值是速度 +int16_t Kinematics::get_motor_speed(uint8_t id) +{ + if (id < 0 || id > 1) + { + return -1; + } + return motor_param[id].motor_speed; +} + + odom_t& Kinematics::get_odom() + { + return odom; + } + +void Kinematics::TransAngleInPI(float angle,float& out_angle) +{ + if(angle>PI) + { + out_angle -= 2*PI; + }else if (angle<-PI) + { + out_angle += 2*PI; + } +} + +void Kinematics::update_odom(uint16_t dt) +{ + float dt_s = float(dt)/1000.0; // ms -> s + // 获取实时的角速度和线速度呢？我们拿左右轮实时的速度，进行运动学正解 + this->kinematics_forward(motor_param[0].motor_speed,motor_param[1].motor_speed,&odom.linear_speed,&odom.angular_speed); + // 计算里程计信息 + odom.linear_speed = odom.linear_speed/1000.0; // 转换成米每秒 + + // 角度积分 + odom.angle += odom.angular_speed*dt_s; + TransAngleInPI(odom.angle,odom.angle); + // 计算机器人行走的距离（沿自身前进方向的） + float delta_distance = odom.linear_speed * dt_s; + // 分解到X轴和Y轴 + odom.x += delta_distance * std::cos(odom.angle); + odom.y += delta_distance * std::sin(odom.angle); +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.h b/chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.h new file mode 100644 index 0000000..6bc6db6 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/lib/Kinematics/Kinematics.h @@ -0,0 +1,57 @@ +#ifndef __KINEMATICS_H__ +#define __KINEMATICS_H__ + +#include "Arduino.h" + +typedef struct +{ + float per_pulse_distance; // 每个脉冲的前进的距离 + int16_t motor_speed; // 单位用mm/s + int64_t last_encoder_ticks; // 上一次点击的编码器读数 +} motor_param_t; + + +typedef struct { + float x; + float y; + float angle; + float linear_speed; + float angular_speed; +}odom_t; + +/** + * 1. 运动学正逆解（两个轮子的实时速度->当前实时的角速度和线速度 / 当前目标的角速度和线速度->两个轮子的目标速度） + * + */ +class Kinematics +{ +private: + /* data */ + motor_param_t motor_param[2]; + int16_t delta_ticks[2] = {0, 0}; // 用于存储上一次读取的编码器数值 + uint64_t last_update_time = 0; // 用于存储上一次更新电机速度的时间,计算速度的时候使用 + float wheel_distance = 0.0; // 两个轮子之间的距离 + odom_t odom; // 用于存储里程计信息 +public: + Kinematics(/* args */) = default; + ~Kinematics() = default; + + odom_t& get_odom(); + void update_odom(uint16_t dt); + void TransAngleInPI(float angle,float& out_angle); + + void set_wheel_distance(float distance); // 设置轮子的间距 + + void set_motor_param(uint8_t id,float per_pluse_distance); + // 运动学正解,将左右轮的速度转换成线速度和角速度 + void kinematics_forward(float left_speed,float right_speed,float* out_linear_speed,float* out_angular_speed); + // 运动学逆解，将线速度和角速度转换成左右轮的速度 + void kinematics_inverse(float linear_speed,float angular_speed,float* out_left_speed,float* out_right_speed); + // 更新点击速度和编码器数据 + void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick); + // 获取电机速度,返回值是速度 + int16_t get_motor_speed(uint8_t id); + +}; + +#endif // __KINEMATICS_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.cpp b/chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.cpp new file mode 100644 index 0000000..c5b5d77 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.cpp @@ -0,0 +1,64 @@ +#include "Arduino.h" +#include "PidController.h" + +// 构造函数，传入三个PID参数 +PidController::PidController(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +float PidController::update(float current) +{ + error_ = target_ - current; // 计算error + + error_sum_ += error_; // 计算error_sum,同时限制积分上下限 + if (error_sum_ > intergral_up_) + error_sum_ = intergral_up_; + if (error_sum_ < -1 * intergral_up_) + error_sum_ = -1 * intergral_up_; + + derror_ = prev_error_ - error_; // 计算误差变化率 + prev_error_ = error_; // 方便下次计算使用 + + float output = kp_ * error_ + ki_ * error_sum_ + kd_ * derror_; + + if (output > out_max_) + output = out_max_; + if (output < out_min_) + output = out_min_; + + return output; +} + +void PidController::update_target(float target) +{ + target_ = target; +} + +void PidController::update_pid(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +void PidController::reset() +{ + error_sum_ = 0; + prev_error_ = 0; + error_ = 0; + derror_ = 0; + kp_ = 0; + ki_ = 0; + kd_ = 0; + intergral_up_ = 2500; + out_min_ = 0; + out_max_ = 0; +} +void PidController::out_limit(float min, float max) +{ + out_min_ = min; + out_max_ = max; +} diff --git a/chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.h b/chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.h new file mode 100644 index 0000000..d184c90 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.h @@ -0,0 +1,33 @@ +#ifndef __PID_CONTROLLER_H__ +#define __PID_CONTROLLER_H__ + +class PidController +{ +public: + PidController() = default; + PidController(float kp, float ki, float kd); + +private: + // PID 参数，可以调节的 + float target_; + float out_min_; + float out_max_; + float kp_; + float ki_; + float kd_; + float intergral_up_ = 2500; // 积分上限 + // pid 中间过程值 + float error_; + float error_sum_; + float derror_; + float prev_error_; + +public: + float update(float current); // 提供当前值，返回下次输出值，也就是PID的结果 + void update_target(float target); // 更新目标值 + void update_pid(float kp, float ki, float kd); // 更新PID参数 + void reset(); // 重置PID + void out_limit(float min, float max); // 设置输出限制 +}; + +#endif // __PID_CONTROLLER_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3/lib/README b/chapt9/fishbot_motion_control_9.3/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/fishbot_motion_control_9.3/platformio.ini b/chapt9/fishbot_motion_control_9.3/platformio.ini new file mode 100644 index 0000000..8dfac39 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/platformio.ini @@ -0,0 +1,16 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino +lib_deps = + http://github.fishros.org/https://github.com/fishros/Esp32McpwmMotor.git + http://github.fishros.org/https://github.com/fishros/Esp32PcntEncoder.git \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.3/src/main.cpp b/chapt9/fishbot_motion_control_9.3/src/main.cpp new file mode 100644 index 0000000..66f6f6a --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/src/main.cpp @@ -0,0 +1,54 @@ +#include +#include +#include +#include +#include + + +Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 +Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 +PidController pid_controller[2]; +Kinematics kinematics; + +float target_linear_speed = 20.0; // 单位 毫米每秒 +float target_angular_speed = 0.1; // 单位 弧度每秒 +float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 +float out_right_speed = 0.0; +// v=w*r +// r = v/w = 0.05/0.1 = 0.5 0.02/0.1 = 0.2 m +void setup() +{ + // 初始化串口 + Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 + // 初始化电机驱动器 + motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23 + motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13 + // 初始化编码器 + encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 + encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 + // 初始化PID控制器的参数 + pid_controller[0].update_pid(0.625, 0.125, 0.0); + pid_controller[1].update_pid(0.625, 0.125, 0.0); + pid_controller[0].out_limit(-100, 100); + pid_controller[1].out_limit(-100, 100); + // 初始化运动学参数 + kinematics.set_wheel_distance(175); // mm + kinematics.set_motor_param(0, 0.105805); + kinematics.set_motor_param(1, 0.105805); + // 测试下运动学逆解 + kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); + Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); + pid_controller[0].update_target(out_left_speed); + pid_controller[1].update_target(out_right_speed); +} + +void loop() +{ + delay(10); // 等待10毫秒 + kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); // 记得调用更新电机速度函数 + motor.updateMotorSpeed(0, pid_controller[0].update( + kinematics.get_motor_speed(0))); + motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(0))); + // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); + Serial.printf("x,y,yaw=%f,%f,%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); +} diff --git a/chapt9/fishbot_motion_control_9.3/test/README b/chapt9/fishbot_motion_control_9.3/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/fishbot_motion_control_9.3/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/fishbot_motion_control_9.4.1/.gitignore b/chapt9/fishbot_motion_control_9.4.1/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/chapt9/fishbot_motion_control_9.4.1/.vscode/c_cpp_properties.json b/chapt9/fishbot_motion_control_9.4.1/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..ecb2141 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/.vscode/c_cpp_properties.json @@ -0,0 +1,531 @@ +// +// !!! WARNING !!! AUTO-GENERATED FILE! +// PLEASE DO NOT MODIFY IT AND USE "platformio.ini": +// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags +// +{ + "configurations": [ + { + "name": "PlatformIO", + "includePath": [ + "/home/fishros/chapt9/fishbot_motion_control/include", + "/home/fishros/chapt9/fishbot_motion_control/src", + "/home/fishros/chapt9/fishbot_motion_control/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/libmicroros/include", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino/wifi", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/home/fishros/chapt9/fishbot_motion_control/include", + "/home/fishros/chapt9/fishbot_motion_control/src", + "/home/fishros/chapt9/fishbot_motion_control/lib/PidController", + "/home/fishros/chapt9/fishbot_motion_control/lib/Kinematics", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/libmicroros/include", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/micro_ros_platformio/platform_code/arduino/wifi", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32PcntEncoder", + "/home/fishros/chapt9/fishbot_motion_control/.pio/libdeps/fishbot/Esp32McpwmMotor", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/port/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hw_support/port/esp32/private_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/heap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/log/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/include/apps/sntp", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/lwip/src/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/lwip/port/esp32/include/arch", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/soc/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/hal/platform_port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rom/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/soc", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_system/port/public_compat", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/xtensa/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/driver/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_pm/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ringbuf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/efuse/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/vfs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_wifi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_event/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_netif/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_eth/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcpip_adapter/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_phy/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_ipc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_trace/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_timer/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/mbedtls/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mbedtls/esp_crt_bundle/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/app_update/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spi_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bootloader_support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nvs_flash/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/pthread/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_gdbstub/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espcoredump/include/port/xtensa", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wpa_supplicant/esp_supplicant/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ieee802154/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/console", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/asio/asio/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/asio/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/osi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/include/esp32/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/blufi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/common/btc/profile/esp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/host/bluedroid/api/include/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_common/tinycrypt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_core/storage", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/btc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/mesh_models/server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/core/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api/models/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/bt/esp_ble_mesh/api", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cbor/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/unity/unity/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/cmock/CMock/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/coap/libcoap/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/nghttp/nghttp2/lib/includes", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-tls/esp-tls-crypto", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_adc_cal/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_hid/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/tcp_transport/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_http_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_ota/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_https_server/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_lcd/interface", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protobuf-c/protobuf-c", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/common", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/security", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/protocomm/include/transports", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mdns/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_local_ctrl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/sdmmc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_serial_slave_link/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_websocket_client/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/expat/expat/lib", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/expat/port/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wear_levelling/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/diskio", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/vfs", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fatfs/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freemodbus/freemodbus/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/idf_test/include/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/jsmn/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json/cJSON", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/libsodium/src/libsodium/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/libsodium/port_include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/mqtt/esp-mqtt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/openssl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/perfmon/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/spiffs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ulp/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/wifi_provisioning/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rmaker_common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_diagnostics/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/rtc_store/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_insights/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_parser/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/json_generator/upstream", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_schedule/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp_secure_cert_mgr/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_rainmaker/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/gpio_button/button/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/qrcode/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/ws2812_led", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp_littlefs/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/tool", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/typedef", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/image", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/math", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/nn", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_harris/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman_nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/nuttall/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/flat_top/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/iir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fir/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/math/sqrt/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/add/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/addc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mulc/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/sub/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/fft/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dct/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/conv/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/common/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/matrix/mul/test/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/kalman/ekf_imu13states/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/fb_gfx/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/cores/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/variants/esp32", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/BluetoothSerial/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/EEPROM/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESP32/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ethernet/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FFat/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/FS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdateServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/I2S/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Insights/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/NetBIOS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Preferences/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/USB/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Update/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src", + "/home/fishros/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", + "" + ] + }, + "defines": [ + "PLATFORMIO=60116", + "ARDUINO_ESP32_DEV", + "HAVE_CONFIG_H", + "MBEDTLS_CONFIG_FILE=\"mbedtls/esp_config.h\"", + "UNITY_INCLUDE_CONFIG_H", + "WITH_POSIX", + "_GNU_SOURCE", + "IDF_VER=\"v4.4.7-dirty\"", + "ESP_PLATFORM", + "_POSIX_READER_WRITER_LOCKS", + "ARDUINO_ARCH_ESP32", + "ESP32", + "F_CPU=240000000L", + "ARDUINO=10812", + "ARDUINO_VARIANT=\"esp32\"", + "ARDUINO_BOARD=\"Espressif ESP32 Dev Module\"", + "ARDUINO_PARTITION_default", + "MICRO_ROS_TRANSPORT_ARDUINO_WIFI=1", + "MICRO_ROS_DISTRO_HUMBLE =1", + "" + ], + "cStandard": "gnu99", + "cppStandard": "gnu++11", + "compilerPath": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc", + "compilerArgs": [ + "-mlongcalls", + "" + ] + } + ], + "version": 4 +} diff --git a/chapt9/fishbot_motion_control_9.4.1/.vscode/extensions.json b/chapt9/fishbot_motion_control_9.4.1/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/chapt9/fishbot_motion_control_9.4.1/.vscode/launch.json b/chapt9/fishbot_motion_control_9.4.1/.vscode/launch.json new file mode 100644 index 0000000..30e099f --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/.vscode/launch.json @@ -0,0 +1,44 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PlatformIO Debugging Solution +// +// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html +// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/home/fishros/chapt9/fishbot_motion_control/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/home/fishros/chapt9/fishbot_motion_control/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "/home/fishros/chapt9/fishbot_motion_control/.pio/build/fishbot/firmware.elf", + "projectEnvName": "fishbot", + "toolchainBinDir": "/home/fishros/.platformio/packages/toolchain-xtensa-esp32/bin", + "internalConsoleOptions": "openOnSessionStart", + "loadMode": "manual" + } + ] +} diff --git a/chapt9/fishbot_motion_control_9.4.1/.vscode/settings.json b/chapt9/fishbot_motion_control_9.4.1/.vscode/settings.json new file mode 100644 index 0000000..1be854f --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "cmath": "cpp" + } +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.4.1/README.md b/chapt9/fishbot_motion_control_9.4.1/README.md new file mode 100644 index 0000000..270da5d --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/README.md @@ -0,0 +1,4 @@ +一圈 1930 个脉冲数 +轮子的直径是 65mm +轮子转一圈是 65*PI +那么一个脉冲数，相当于走了多远距离 65*PI/1930 0.105805 mm \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.4.1/include/README b/chapt9/fishbot_motion_control_9.4.1/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.cpp b/chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.cpp new file mode 100644 index 0000000..3deae23 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.cpp @@ -0,0 +1,89 @@ +#include "Kinematics.h" + +// 设置轮子的间距 +void Kinematics::set_wheel_distance(float distance) +{ + wheel_distance = distance; +} + +void Kinematics::set_motor_param(uint8_t id, float per_pluse_distance) +{ + motor_param[id].per_pulse_distance = per_pluse_distance; +} + + +// 运动学正解,将左右轮的速度转换成线速度和角速度 +void Kinematics::kinematics_forward(float left_speed, float right_speed, float *out_linear_speed, float *out_angular_speed) +{ + *out_linear_speed = (left_speed + right_speed) / 2; + *out_angular_speed = (right_speed - left_speed) / wheel_distance; +} + + +// 运动学逆解，将线速度和角速度转换成左右轮的速度 +void Kinematics::kinematics_inverse(float linear_speed, float angular_speed, float *out_left_speed, float *out_right_speed) +{ + *out_left_speed = linear_speed - angular_speed * wheel_distance / 2; + *out_right_speed = linear_speed + angular_speed * wheel_distance / 2; +} + +// 输入：左右轮脉冲数，当前时间，输出更新电机速度和编码器数据 +void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick) +{ + int16_t dt = current_time - last_update_time; // ms + delta_ticks[0] = left_tick - motor_param[0].last_encoder_ticks; + delta_ticks[1] = right_tick - motor_param[1].last_encoder_ticks; + + motor_param[0].motor_speed = (delta_ticks[0] * 105.805) / dt; + motor_param[1].motor_speed = (delta_ticks[1] * 105.805) / dt; + + // 为了下次还可以正常的计算速度 + motor_param[0].last_encoder_ticks = left_tick; + motor_param[1].last_encoder_ticks = right_tick; + last_update_time = current_time; + + update_odom(dt); +} +// 获取电机速度,返回值是速度 +int16_t Kinematics::get_motor_speed(uint8_t id) +{ + if (id < 0 || id > 1) + { + return -1; + } + return motor_param[id].motor_speed; +} + + odom_t& Kinematics::get_odom() + { + return odom; + } + +void Kinematics::TransAngleInPI(float angle,float& out_angle) +{ + if(angle>PI) + { + out_angle -= 2*PI; + }else if (angle<-PI) + { + out_angle += 2*PI; + } +} + +void Kinematics::update_odom(uint16_t dt) +{ + float dt_s = float(dt)/1000.0; // ms -> s + // 获取实时的角速度和线速度呢？我们拿左右轮实时的速度，进行运动学正解 + this->kinematics_forward(motor_param[0].motor_speed,motor_param[1].motor_speed,&odom.linear_speed,&odom.angular_speed); + // 计算里程计信息 + odom.linear_speed = odom.linear_speed/1000.0; // 转换成米每秒 + + // 角度积分 + odom.angle += odom.angular_speed*dt_s; + TransAngleInPI(odom.angle,odom.angle); + // 计算机器人行走的距离（沿自身前进方向的） + float delta_distance = odom.linear_speed * dt_s; + // 分解到X轴和Y轴 + odom.x += delta_distance * std::cos(odom.angle); + odom.y += delta_distance * std::sin(odom.angle); +} \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.h b/chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.h new file mode 100644 index 0000000..6bc6db6 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/lib/Kinematics/Kinematics.h @@ -0,0 +1,57 @@ +#ifndef __KINEMATICS_H__ +#define __KINEMATICS_H__ + +#include "Arduino.h" + +typedef struct +{ + float per_pulse_distance; // 每个脉冲的前进的距离 + int16_t motor_speed; // 单位用mm/s + int64_t last_encoder_ticks; // 上一次点击的编码器读数 +} motor_param_t; + + +typedef struct { + float x; + float y; + float angle; + float linear_speed; + float angular_speed; +}odom_t; + +/** + * 1. 运动学正逆解（两个轮子的实时速度->当前实时的角速度和线速度 / 当前目标的角速度和线速度->两个轮子的目标速度） + * + */ +class Kinematics +{ +private: + /* data */ + motor_param_t motor_param[2]; + int16_t delta_ticks[2] = {0, 0}; // 用于存储上一次读取的编码器数值 + uint64_t last_update_time = 0; // 用于存储上一次更新电机速度的时间,计算速度的时候使用 + float wheel_distance = 0.0; // 两个轮子之间的距离 + odom_t odom; // 用于存储里程计信息 +public: + Kinematics(/* args */) = default; + ~Kinematics() = default; + + odom_t& get_odom(); + void update_odom(uint16_t dt); + void TransAngleInPI(float angle,float& out_angle); + + void set_wheel_distance(float distance); // 设置轮子的间距 + + void set_motor_param(uint8_t id,float per_pluse_distance); + // 运动学正解,将左右轮的速度转换成线速度和角速度 + void kinematics_forward(float left_speed,float right_speed,float* out_linear_speed,float* out_angular_speed); + // 运动学逆解，将线速度和角速度转换成左右轮的速度 + void kinematics_inverse(float linear_speed,float angular_speed,float* out_left_speed,float* out_right_speed); + // 更新点击速度和编码器数据 + void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick); + // 获取电机速度,返回值是速度 + int16_t get_motor_speed(uint8_t id); + +}; + +#endif // __KINEMATICS_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.cpp b/chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.cpp new file mode 100644 index 0000000..c5b5d77 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.cpp @@ -0,0 +1,64 @@ +#include "Arduino.h" +#include "PidController.h" + +// 构造函数，传入三个PID参数 +PidController::PidController(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +float PidController::update(float current) +{ + error_ = target_ - current; // 计算error + + error_sum_ += error_; // 计算error_sum,同时限制积分上下限 + if (error_sum_ > intergral_up_) + error_sum_ = intergral_up_; + if (error_sum_ < -1 * intergral_up_) + error_sum_ = -1 * intergral_up_; + + derror_ = prev_error_ - error_; // 计算误差变化率 + prev_error_ = error_; // 方便下次计算使用 + + float output = kp_ * error_ + ki_ * error_sum_ + kd_ * derror_; + + if (output > out_max_) + output = out_max_; + if (output < out_min_) + output = out_min_; + + return output; +} + +void PidController::update_target(float target) +{ + target_ = target; +} + +void PidController::update_pid(float kp, float ki, float kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +void PidController::reset() +{ + error_sum_ = 0; + prev_error_ = 0; + error_ = 0; + derror_ = 0; + kp_ = 0; + ki_ = 0; + kd_ = 0; + intergral_up_ = 2500; + out_min_ = 0; + out_max_ = 0; +} +void PidController::out_limit(float min, float max) +{ + out_min_ = min; + out_max_ = max; +} diff --git a/chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.h b/chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.h new file mode 100644 index 0000000..d184c90 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/lib/PidController/PidController.h @@ -0,0 +1,33 @@ +#ifndef __PID_CONTROLLER_H__ +#define __PID_CONTROLLER_H__ + +class PidController +{ +public: + PidController() = default; + PidController(float kp, float ki, float kd); + +private: + // PID 参数，可以调节的 + float target_; + float out_min_; + float out_max_; + float kp_; + float ki_; + float kd_; + float intergral_up_ = 2500; // 积分上限 + // pid 中间过程值 + float error_; + float error_sum_; + float derror_; + float prev_error_; + +public: + float update(float current); // 提供当前值，返回下次输出值，也就是PID的结果 + void update_target(float target); // 更新目标值 + void update_pid(float kp, float ki, float kd); // 更新PID参数 + void reset(); // 重置PID + void out_limit(float min, float max); // 设置输出限制 +}; + +#endif // __PID_CONTROLLER_H__ \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.4.1/lib/README b/chapt9/fishbot_motion_control_9.4.1/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/chapt9/fishbot_motion_control_9.4.1/platformio.ini b/chapt9/fishbot_motion_control_9.4.1/platformio.ini new file mode 100644 index 0000000..6915ea3 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/platformio.ini @@ -0,0 +1,19 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html +[env:fishbot] +platform = espressif32 +board = esp32dev +framework = arduino +board_microros_transport = wifi +monitor_speed = 115200 +lib_deps = + http://github.fishros.org/https://github.com/fishros/Esp32McpwmMotor.git + http://github.fishros.org/https://github.com/fishros/Esp32PcntEncoder.git + https://gitee.com/ohhuo/micro_ros_platformio.git \ No newline at end of file diff --git a/chapt9/fishbot_motion_control_9.4.1/src/main.cpp b/chapt9/fishbot_motion_control_9.4.1/src/main.cpp new file mode 100644 index 0000000..06e9861 --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/src/main.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include +#include + +// 引入Microros和wifi相关的库 +#include +#include +#include +#include +#include + +// 声明一些相关的结构体对象 +rcl_allocator_t allocator; // 内存分配器，用于动态内存分配管理 +rclc_support_t support; // 用于存储时钟，内存分配器和上下文，用于提供支持 +rclc_executor_t executor; // 执行器，用于管理订阅和计时器回调的执行 +rcl_node_t node; // 节点，用于创建节点 + +// 单独创建一个任务运行 micro-ROS 相当于一个线程 +void microros_task(void* args) +{ + // 1.设置传输协议并延迟一段时间等待设置的完成 + IPAddress agent_ip; + agent_ip.fromString("192.168.1.103"); // 设置agent的IP地址 + set_microros_wifi_transports("fishros","88888888",agent_ip,8888); // 设置传输协议 + delay(3000); // 等待2秒,等待WIFI连接 + // 2.初始化内存分配器 + allocator = rcl_get_default_allocator(); // 获取默认的内存分配器 + // 3.初始化支持 + rclc_support_init(&support,0,NULL,&allocator); // 初始化支持 + // 4.初始化节点 + rclc_node_init_default(&node,"fishbot_motion_control","",&support); // 初始化节点 + // 5.初始化执行器 + unsigned int num_handles = 0; // 订阅和计时器的回调数量,注意这是一个要改的参数 + rclc_executor_init(&executor,&support.context,num_handles,&allocator); // 初始化执行器 + // 循环执行器 + rclc_executor_spin(&executor); +} + +Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 +Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 +PidController pid_controller[2]; +Kinematics kinematics; + + +float target_linear_speed = 0.0; // 单位 毫米每秒 +float target_angular_speed = 0.0; // 单位 弧度每秒 +float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 +float out_right_speed = 0.0; + +// v=w*r +// r = v/w = 0.05/0.1 = 0.5 0.02/0.1 = 0.2 m +void setup() +{ + // 初始化串口 + Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 + // 初始化电机驱动器 + motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23 + motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13 + // 初始化编码器 + encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 + encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 + // 初始化PID控制器的参数 + pid_controller[0].update_pid(0.625, 0.125, 0.0); + pid_controller[1].update_pid(0.625, 0.125, 0.0); + pid_controller[0].out_limit(-100, 100); + pid_controller[1].out_limit(-100, 100); + // 初始化运动学参数 + kinematics.set_wheel_distance(175); // mm + kinematics.set_motor_param(0, 0.105805); + kinematics.set_motor_param(1, 0.105805); + // 测试下运动学逆解 + kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); + Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); + pid_controller[0].update_target(out_left_speed); + pid_controller[1].update_target(out_right_speed); + + // 创建一个任务运行 micro-ROS + xTaskCreatePinnedToCore(microros_task, "microros_task", 10240, NULL, 1, NULL, 1); +} + +void loop() +{ + delay(10); // 等待10毫秒 + kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); // 记得调用更新电机速度函数 + motor.updateMotorSpeed(0, pid_controller[0].update( + kinematics.get_motor_speed(0))); + motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1))); + // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); + // Serial.printf("x,y,yaw=%f,%f,%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); +} + diff --git a/chapt9/fishbot_motion_control_9.4.1/test/README b/chapt9/fishbot_motion_control_9.4.1/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/chapt9/fishbot_motion_control_9.4.1/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt b/chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt index a3efba1..4a70c92 100644 --- a/chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt +++ b/chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt @@ -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() diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py b/chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py index 71fffdd..fef387e 100644 --- a/chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py +++ b/chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py @@ -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, diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/map/room.pgm b/chapt9/fishbot_ws/src/fishbot_bringup/map/room.pgm new file mode 100644 index 0000000000000000000000000000000000000000..47efac8afeb6110e19abb34b5a1ca7c54c68f486 GIT binary patch literal 6763 zcmds(YmOTs5QOKqPmv=;k*p}U$i;o7QM#&{<}uhiJo0DR)xco*x+og#zt6{?pFe&- zAHSZ@$7{YE^HV?01owZjU*~?Zi08*O&3x!SYzH3(K8$*vTWtoG(AQqBv>F-zTet5p z2SjD9eVo~;%N&5gboy5EX(fSAnSO~s(Lu>XL%Wr2q6LkokvE~C(h7V|WfMB#GI)<4 zho#)5yw*h@H8{&&bh_ad(pRsJ0#!99GPj*l@$iDvvZK@{2X^A5)Z5ayoOf?nv}KYl zH;RF~3C97ohM3cB5W_22Q+wRROLZ62bQ%|58Qdhy>k8)a7@>kAuGmM9YhNW}%q(4k z-Gi_qthSy!cD!Xb4X((*@#e9R>(>Oy63?f!&HKRl$T;XNxOm5B69+?*1QxLYJ97$+ zo1iEySqEp7jFl!Rb{_2u+4?Z7Slyjw!f>n{PF*RYY72@ndfp&|RSYA#PsUZ&{W9znb~ZYynP0yYhflOJ40^ssyd9A=`+joQh+jM|yz~GP7$W&ctwVcw zmF~zS-H`n9Q;h5Ifu2A@ISS^wy0~7xeV!K_Gz&nTfJ1mn05s-AgJikkFyr7~ zmEdJNg}YM&X_{yW#zQT^24oa^G9#KG`>mvub~2GfCp_w*LWQnV-XanWp;o~n#$C28 zkt-+C9xwsdrd4E>7L@HfeCf;N#6*NU^WTnPN*KdzreyA1NM2!M;qt>YqEM@z7dc~o zIGT7z8&_fxv@By}P?80i+#pN*{kBAdj|lLi7V4x?y_lwR4IaUnn_KsFRwF%oMeYl( z#t*yz9r&xz86R?VnTx0AU3|30#cXH|vcJ#X${{NNR9x7xrkDIcR5=&Dg@);7CU3=> o9-KnR+Q$tNH*tE&Zzmex6fgd$?bGeN?B~i4aKo8?^zYXD4><-y1poj5 literal 0 HcmV?d00001 diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/map/room.yaml b/chapt9/fishbot_ws/src/fishbot_bringup/map/room.yaml new file mode 100644 index 0000000..7acbd97 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_bringup/map/room.yaml @@ -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 \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/package.xml b/chapt9/fishbot_ws/src/fishbot_bringup/package.xml index 8f7d5d7..888ca26 100644 --- a/chapt9/fishbot_ws/src/fishbot_bringup/package.xml +++ b/chapt9/fishbot_ws/src/fishbot_bringup/package.xml @@ -4,7 +4,7 @@ fishbot_bringup 0.0.0 TODO: Package description - fishros + fishros TODO: License declaration ament_cmake diff --git a/chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt b/chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt index 80f85e1..1f1d5e4 100644 --- a/chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt +++ b/chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt @@ -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() diff --git a/chapt9/fishbot_ws/src/fishbot_description/package.xml b/chapt9/fishbot_ws/src/fishbot_description/package.xml index 15178d3..1a7fecf 100644 --- a/chapt9/fishbot_ws/src/fishbot_description/package.xml +++ b/chapt9/fishbot_ws/src/fishbot_description/package.xml @@ -4,7 +4,7 @@ fishbot_description 0.0.0 TODO: Package description - fishros + fishros TODO: License declaration ament_cmake diff --git a/chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf b/chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf index 9ebe729..6dcc7cc 100644 --- a/chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf +++ b/chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf @@ -14,6 +14,7 @@ + diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt b/chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt index 2310073..956c782 100644 --- a/chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt +++ b/chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt @@ -30,4 +30,5 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME} ) + ament_package() diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml b/chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml index 5f4b08f..81dfc98 100644 --- a/chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml +++ b/chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml @@ -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 diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.pgm b/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.pgm index 4af93a954c8a000561ecc46b91cc230d15c62826..47efac8afeb6110e19abb34b5a1ca7c54c68f486 100644 GIT binary patch literal 6763 zcmds(YmOTs5QOKqPmv=;k*p}U$i;o7QM#&{<}uhiJo0DR)xco*x+og#zt6{?pFe&- zAHSZ@$7{YE^HV?01owZjU*~?Zi08*O&3x!SYzH3(K8$*vTWtoG(AQqBv>F-zTet5p z2SjD9eVo~;%N&5gboy5EX(fSAnSO~s(Lu>XL%Wr2q6LkokvE~C(h7V|WfMB#GI)<4 zho#)5yw*h@H8{&&bh_ad(pRsJ0#!99GPj*l@$iDvvZK@{2X^A5)Z5ayoOf?nv}KYl zH;RF~3C97ohM3cB5W_22Q+wRROLZ62bQ%|58Qdhy>k8)a7@>kAuGmM9YhNW}%q(4k z-Gi_qthSy!cD!Xb4X((*@#e9R>(>Oy63?f!&HKRl$T;XNxOm5B69+?*1QxLYJ97$+ zo1iEySqEp7jFl!Rb{_2u+4?Z7Slyjw!f>n{PF*RYY72@ndfp&|RSYA#PsUZ&{W9znb~ZYynP0yYhflOJ40^ssyd9A=`+joQh+jM|yz~GP7$W&ctwVcw zmF~zS-H`n9Q;h5Ifu2A@ISS^wy0~7xeV!K_Gz&nTfJ1mn05s-AgJikkFyr7~ zmEdJNg}YM&X_{yW#zQT^24oa^G9#KG`>mvub~2GfCp_w*LWQnV-XanWp;o~n#$C28 zkt-+C9xwsdrd4E>7L@HfeCf;N#6*NU^WTnPN*KdzreyA1NM2!M;qt>YqEM@z7dc~o zIGT7z8&_fxv@By}P?80i+#pN*{kBAdj|lLi7V4x?y_lwR4IaUnn_KsFRwF%oMeYl( z#t*yz9r&xz86R?VnTx0AU3|30#cXH|vcJ#X${{NNR9x7xrkDIcR5=&Dg@);7CU3=> o9-KnR+Q$tNH*tE&Zzmex6fgd$?bGeN?B~i4aKo8?^zYXD4><-y1poj5 literal 15671 zcmeI1Taw%$3`P5UR*@xSCe{1M#(PVy=t>A75PlNdAEr``z#tu6VUN4>_x<(#{`vL& z`_I?U`~CHK+M~duz@xyUz@xyUz@xym3cRij--}LIcOrXzUfO!Z=gudZn_;O&ug|3I zTv>Y25)BMg+y>JP>X-ye_obY% z!+OD6=iscHw(9ArN59Hx5I7|#-8z8Is+en49Sg34N7GxCS_g`zk8mjs|D{kK?f(;3 zAx1AL#!7dJ(paCM!_BLltE>FLz{Hwt-3Ikyy?}*WYyO^O@2;W)xHvtUd+W5@&=U&m}wC#a}`v>xYA>qN?P9Gi%dwtxUxl=n(jHx#&C()xhgDrLy=QP zeY5>Dk5|%|%Q0NykN96T1CQ%ThaPi9`MGU0=bQ^@r82sQ>;P9p5YCZYv0el@(c48-%9v~PgSF4aPyyIU zTr-K{khNfEaG{Pfih-&Dy8~AY79Z8>zKMog?#EfZNUjP0T#LQW$*$YuopGAo`;ndD zSFXC2aUn-?(c9_@caTgoM!%B_Cpna+om@jjAQf2bK28uYMQahl8QM__(X(l9saVYA zpEm^ynPKpmN#5O~aq}fuCk~}}jHUn;M~SzV%jIi{8d9a9lVf>ANLsN%pu%+dOe0i$e>P>u8J}TuB?7yxvM!w~(k% zL#lY$Au33m$ylAgcy8gz1LBe+e{$TVaD+uC3!isufVyEJS*;s#L9RRMMO5_Wbi?z- zzuDfA>z;LFxsKMce#(rHAJ0|1+zf2El~%jMVzg>Q!m6jMRojL^nA!oOZq~d%+c`$b%}5-7iNRzxJ1>hdI=Ts0h4mcAaOt|ZmYSxU{dWhU@8%<)mXx% ziB?=m5?vc8R85H=3qzM@xNKdv(vKCqj|HO-6RnE2o(u9@V>d0AEvPKq`Kd%@LYU>Y zfuqVG7^+iy+$5S}t%Ma8n^qe0VAqaY$*aJ0-0@cw{6a^I^O7u+P}67UDUf$;G5|y_ z`+MZ2R&IJ)nHOf6Km@j-SbWFlSfN4StQB1>^>U9xWH*>AWbu<0(do(>5voAhBER{R kk>gUH3zVvpbzJqu3&(fmx=@lQ^rOI|z@xyUzfishbot_navigation2 0.0.0 TODO: Package description - fishros + fishros TODO: License declaration ament_cmake diff --git a/chapt9/fishbot_ws/src/micro-ROS-Agent b/chapt9/fishbot_ws/src/micro-ROS-Agent new file mode 160000 index 0000000..30377bb --- /dev/null +++ b/chapt9/fishbot_ws/src/micro-ROS-Agent @@ -0,0 +1 @@ +Subproject commit 30377bbd86ff7ea93ca69a3b37997fd235385e1f diff --git a/chapt9/fishbot_ws/src/micro_ros_msgs b/chapt9/fishbot_ws/src/micro_ros_msgs new file mode 160000 index 0000000..10be4d0 --- /dev/null +++ b/chapt9/fishbot_ws/src/micro_ros_msgs @@ -0,0 +1 @@ +Subproject commit 10be4d005fbc7d8dd60dbb213b65f4171419bfe9 diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/.gitignore b/chapt9/fishbot_ws/src/ros_serial2wifi/.gitignore new file mode 100644 index 0000000..d18e8c4 --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/.gitignore @@ -0,0 +1,3 @@ +build/ +log/ +install/ diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/package.xml b/chapt9/fishbot_ws/src/ros_serial2wifi/package.xml new file mode 100644 index 0000000..47c73c3 --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/package.xml @@ -0,0 +1,18 @@ + + + + ros_serail2wifi + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/resource/ros_serail2wifi b/chapt9/fishbot_ws/src/ros_serial2wifi/resource/ros_serail2wifi new file mode 100644 index 0000000..e69de29 diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/__init__.py b/chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/tcpserver.py b/chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/tcpserver.py new file mode 100644 index 0000000..9cc48fc --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/ros_serail2wifi/tcpserver.py @@ -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() diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/setup.cfg b/chapt9/fishbot_ws/src/ros_serial2wifi/setup.cfg new file mode 100644 index 0000000..9ede5ad --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ros_serail2wifi +[install] +install_scripts=$base/lib/ros_serail2wifi diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/setup.py b/chapt9/fishbot_ws/src/ros_serial2wifi/setup.py new file mode 100644 index 0000000..8e66e6e --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/setup.py @@ -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' + ], + }, +) diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_copyright.py b/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_copyright.py @@ -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' diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_flake8.py b/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_flake8.py @@ -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) diff --git a/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_pep257.py b/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt9/fishbot_ws/src/ros_serial2wifi/test/test_pep257.py @@ -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' diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/.gitignore b/chapt9/fishbot_ws/src/ydlidar_ros2/.gitignore new file mode 100644 index 0000000..54cd2dd --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/.gitignore @@ -0,0 +1,3 @@ +build/* +install/* +log/* \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/CMakeLists.txt new file mode 100644 index 0000000..81082c1 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/CMakeLists.txt @@ -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() + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/README.md b/chapt9/fishbot_ws/src/ydlidar_ros2/README.md new file mode 100644 index 0000000..6ccdbad --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/README.md @@ -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 + +
+ +| 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 | + +
+ +## 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) + + + + + + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/FindPackage.cmake.in b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/FindPackage.cmake.in new file mode 100644 index 0000000..42aa83c --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/FindPackage.cmake.in @@ -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 ) + + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfig.cmake.in b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfig.cmake.in new file mode 100644 index 0000000..56a2ad9 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfig.cmake.in @@ -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@) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfigVersion.cmake.in b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfigVersion.cmake.in new file mode 100644 index 0000000..3132080 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PackageConfigVersion.cmake.in @@ -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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PkgConfig.pc.in b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PkgConfig.pc.in new file mode 100644 index 0000000..05e16f2 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/PkgConfig.pc.in @@ -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@ + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/cmake_uninstall.cmake.in b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/cmake_uninstall.cmake.in new file mode 100644 index 0000000..f37a376 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/cmake_uninstall.cmake.in @@ -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") + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_base.cmake b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_base.cmake new file mode 100644 index 0000000..18c6a94 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_base.cmake @@ -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 +# [ OPTIONS ] +# [ ONE_VALUE ] +# [ MULTI_VALUE ] +# REQUIRED +# 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 +# BOARD +# ) +# +# 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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_parse.cmake b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_parse.cmake new file mode 100644 index 0000000..388ee6b --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/common/ydlidar_parse.cmake @@ -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" ) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/install_package.cmake b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/install_package.cmake new file mode 100644 index 0000000..d34d66c --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/install_package.cmake @@ -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 , usually the same as ${PROJECT_NAME} +# LIB_NAME +# VERSION +# INSTALL_HEADERS
+# DESTINATION +# INCLUDE_DIRS +# LINK_LIBS +# LINK_DIRS +# CFLAGS +# CXXFLAGS +# +################################################################################ +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() + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/script_show_final_summary.cmake b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/script_show_final_summary.cmake new file mode 100644 index 0000000..6349c4e --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/cmake/script_show_final_summary.cmake @@ -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 "") diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/config/ydlidar.rviz b/chapt9/fishbot_ws/src/ydlidar_ros2/config/ydlidar.rviz new file mode 100755 index 0000000..7c9e799 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/config/ydlidar.rviz @@ -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: + 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: + 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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/docs/paramters.md b/chapt9/fishbot_ws/src/ydlidar_ros2/docs/paramters.md new file mode 100644 index 0000000..74aa4b9 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/docs/paramters.md @@ -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. diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/docs/ydlidar.md b/chapt9/fishbot_ws/src/ydlidar_ros2/docs/ydlidar.md new file mode 100644 index 0000000..83ab831 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/docs/ydlidar.md @@ -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 + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar.py b/chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar.py new file mode 100644 index 0000000..65b9a10 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar.py @@ -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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar_launch.py b/chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar_launch.py new file mode 100644 index 0000000..e80587f --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/launch/ydlidar_launch.py @@ -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, + ]) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/package.xml b/chapt9/fishbot_ws/src/ydlidar_ros2/package.xml new file mode 100644 index 0000000..fa8bba0 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/package.xml @@ -0,0 +1,31 @@ + + + + ydlidar + 1.4.5 + + YDLidar LiDAR sensor nodes + + Tony + Apache License 2.0 + + ament_cmake + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/params/ydlidar.yaml b/chapt9/fishbot_ws/src/ydlidar_ros2/params/ydlidar.yaml new file mode 100644 index 0000000..ea1b970 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/params/ydlidar.yaml @@ -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 # 无效范围为无穷大 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/CMakeLists.txt new file mode 100755 index 0000000..d3d63eb --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/CMakeLists.txt @@ -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) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/CMakeLists.txt new file mode 100644 index 0000000..e37b425 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/CMakeLists.txt @@ -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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/CMakeLists.txt new file mode 100644 index 0000000..9b61fe6 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/CMakeLists.txt @@ -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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/datatype.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/datatype.h new file mode 100644 index 0000000..71dd625 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/datatype.h @@ -0,0 +1,148 @@ +#ifndef DATATYPE_H_ +#define DATATYPE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(_MSC_VER) +#include +#endif +#include "typedef.h" + +#if !defined(_MSC_VER) +#include +#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 +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_ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/locker.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/locker.h new file mode 100644 index 0000000..e038491 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/locker.h @@ -0,0 +1,359 @@ +#pragma once +#ifdef _WIN32 +#include +#include +#include +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#include +#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 + + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/thread.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/thread.h new file mode 100644 index 0000000..b276b38 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/thread.h @@ -0,0 +1,179 @@ +#pragma once +#include "v8stdint.h" +#include "timer.h" +#ifdef _WIN32 +#include +#include +#include +#include +#else +#include +#include +#endif + +#if defined(__ANDROID__) +#define pthread_cancel(x) 0 +#endif + +#define CLASS_THREAD(c, x) Thread::ThreadCreateObjectFunctor(this) + +namespace ydlidar +{ + namespace core + { + namespace base + { + + class Thread + { + public: + template + static Thread + ThreadCreateObjectFunctor(CLASS *pthis) + { + return createThread(createThreadAux, pthis); + } + + template + static _size_t THREAD_PROC + createThreadAux(void *param) + { + return (static_cast(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(this->_handle), -1)) + { + CloseHandle(reinterpret_cast(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(this->_handle), timeout)) + { + case WAIT_OBJECT_0: + CloseHandle(reinterpret_cast(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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.cpp new file mode 100644 index 0000000..0c5d8a3 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.cpp @@ -0,0 +1,57 @@ +#include "timer.h" +#if defined(_WIN32) +#include +#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(¤t); + + 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(tim.tv_sec) * 1000000000LL + tim.tv_nsec; +#else + struct timeval timeofday; + gettimeofday(&timeofday, NULL); + return static_cast(timeofday.tv_sec) * 1000000000LL + + static_cast(timeofday.tv_usec) * 1000LL; +#endif +} +} +#endif diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.h new file mode 100644 index 0000000..499bb4e --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/timer.h @@ -0,0 +1,52 @@ +#pragma once +#include "v8stdint.h" +#include +#include +#include +#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 +#define delay(x) ::Sleep(x) +#else +#include +#include + +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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/typedef.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/typedef.h new file mode 100644 index 0000000..fe6e685 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/typedef.h @@ -0,0 +1,17 @@ +#ifndef TYPEDEF_H_ +#define TYPEDEF_H_ +#include +#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 +#endif + +#endif // TYPEDEF_H_ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/utils.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/utils.h new file mode 100644 index 0000000..338f0c5 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/utils.h @@ -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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/v8stdint.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/v8stdint.h new file mode 100644 index 0000000..eba142f --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/v8stdint.h @@ -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_ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/ydlidar.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/ydlidar.h new file mode 100644 index 0000000..f86667d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/base/ydlidar.h @@ -0,0 +1,194 @@ +#ifndef YDLIDAR_H_ +#define YDLIDAR_H_ +#include "datatype.h" +#include + +/// 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_ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/CMakeLists.txt new file mode 100644 index 0000000..d38061d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/CMakeLists.txt @@ -0,0 +1,5 @@ +aux_include_directory(. HDRS) +aux_source_directory(. SRCS) +add_to_ydlidar_headers(${HDRS}) +add_to_ydlidar_sources(${SRCS}) + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ChannelDevice.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ChannelDevice.h new file mode 100644 index 0000000..b0a2143 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ChannelDevice.h @@ -0,0 +1,134 @@ +#pragma once +#include + +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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/DriverInterface.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/DriverInterface.h new file mode 100644 index 0000000..a297efd --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/DriverInterface.h @@ -0,0 +1,644 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#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 &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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_config.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_config.h new file mode 100644 index 0000000..df49ed4 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_config.h @@ -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" + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_datatype.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_datatype.h new file mode 100644 index 0000000..b449215 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_datatype.h @@ -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 +#include +#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 points; + int size = 0; //实际点数（固定分辨率时点数与实际点数不符） + // Configuration of scan + LaserConfig config; + int moduleNum = 0; + uint16_t envFlag = 0; //环境标记（目前只针对GS2） +} LaserScan; diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.cpp new file mode 100644 index 0000000..10a03a5 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.cpp @@ -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 +#include + +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; + } +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.h new file mode 100644 index 0000000..316bfa8 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_def.h @@ -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 +#include +#include +#include +#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_ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_help.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_help.h new file mode 100644 index 0000000..b765565 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/common/ydlidar_help.h @@ -0,0 +1,1181 @@ +/********************************************************************* +* Software License Agreement (MIT License) +* +* Copyright © 2020 EAIBOT, Inc. +* 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. +* +* @file ydlidar_help.h * +* @brief LiDAR Help function * +* Details. * +* * +* @author Tony.Yang * +* @email chushuirurong618@eaibot.com * +* @version 1.0.0 * +* @date 2020/02/14 * +* @license MIT * +* * +*----------------------------------------------------------------------------* +* Remark : Description * +*----------------------------------------------------------------------------* +* Change History : * +* | | | * +*----------------------------------------------------------------------------* +* 2020/02/14 | 1.0.0 | Tony | Lidar Help File * +*----------------------------------------------------------------------------* +* * +*********************************************************************/ +#pragma once +#include +#include +#include +#include "DriverInterface.h" + +/** + * @brief ydlidar + */ +namespace ydlidar { +/** + * @brief ydlidar core + */ +namespace core { +using namespace base; +/** + * @brief ydlidar common + */ +namespace common { + +//颜色定义 +#ifndef COLOR + #define COLOFF "\033[0m" ///关闭所有属性 + #define RED "\033[0;31m" ///"\033[显示方式;字体颜色;背景颜色m" + #define GREEN "\033[0;32m" + #define YELLOW "\033[0;33m" + #define BLUE "\033[0;34m" + #define PURPLE "\033[0;35m" +#endif + +//打印系统时间 +#define UNIX_PRINT_TIME \ + time_t currentTime = time(NULL); \ + struct tm *localTime = localtime(¤tTime); \ + printf("[%04d-%02d-%02d %02d:%02d:%02d]", \ + (1900 + localTime->tm_year), \ + (1 + localTime->tm_mon), \ + localTime->tm_mday, \ + localTime->tm_hour, \ + localTime->tm_min, \ + localTime->tm_sec); +#define WIN_PRINT_TIME UNIX_PRINT_TIME +#ifdef _WIN32 + #define PRINT_TIME WIN_PRINT_TIME +#else + #define PRINT_TIME UNIX_PRINT_TIME +#endif +//格式化字符串 +#define FORMAT_STDOUT \ + char buff[1024] = {0}; \ + va_list ap; \ + va_start(ap, fmt); \ + vsprintf(buff, fmt, ap); \ + va_end(ap); \ + printf(buff); \ + printf("\n"); + +//调试 +inline void debug(char* fmt, ...) +{ + printf(GREEN); //设置绿色 + PRINT_TIME + printf("[debug] "); + FORMAT_STDOUT + printf(COLOFF); //恢复默认颜色 + fflush(stdout); +} + +//常规 +inline void info(char* fmt, ...) +{ + PRINT_TIME + printf("[info] "); + FORMAT_STDOUT + fflush(stdout); +} + +//警告 +inline void warn(char* fmt, ...) +{ + printf(YELLOW); //设置黄色 + PRINT_TIME + printf("[warn] "); + FORMAT_STDOUT + printf(COLOFF); //恢复默认颜色 + fflush(stdout); +} + +//错误 +inline void error(char* fmt, ...) +{ + printf(RED); //设置红色 + PRINT_TIME + printf("[error] "); + FORMAT_STDOUT + printf(COLOFF); //恢复默认颜色 + fflush(stdout); +} + +//调试（16进制） +inline void debugh(const uint8_t *data, int size) +{ + if (!data || !size) + return; + printf(GREEN); //设置绿色 + PRINT_TIME + printf("[debug] "); + for (int i=0; i getDefaultSampleRate(int model) +{ + std::vector srs; + + switch (model) + { + case DriverInterface::YDLIDAR_F4: + case DriverInterface::YDLIDAR_T1: + case DriverInterface::YDLIDAR_F2: + srs.push_back(4); + break; + // case DriverInterface::YDLIDAR_S4: + case DriverInterface::YDLIDAR_S2PRO: + srs.push_back(3); + srs.push_back(4); + break; + case DriverInterface::YDLIDAR_G4: + srs.push_back(9); + break; + case DriverInterface::YDLIDAR_X4: + srs.push_back(5); + break; + case DriverInterface::YDLIDAR_G4PRO: + srs.push_back(9); + break; + case DriverInterface::YDLIDAR_F4PRO: + srs.push_back(4); + break; + case DriverInterface::YDLIDAR_R2: + srs.push_back(5); + break; + case DriverInterface::YDLIDAR_G10: + srs.push_back(10); + break; + case DriverInterface::YDLIDAR_S4B: + srs.push_back(4); + break; + case DriverInterface::YDLIDAR_S2: + srs.push_back(3); + break; + case DriverInterface::YDLIDAR_G6: + srs.push_back(18); + break; + case DriverInterface::YDLIDAR_G2A: + case DriverInterface::YDLIDAR_G2B: + srs.push_back(5); + break; + case DriverInterface::YDLIDAR_G2C: + case DriverInterface::YDLIDAR_G4B: + case DriverInterface::YDLIDAR_G4C: + srs.push_back(4); + break; + case DriverInterface::YDLIDAR_G1: + case DriverInterface::YDLIDAR_G5: + srs.push_back(9); + break; + case DriverInterface::YDLIDAR_G7: + srs.push_back(18); + break; + case DriverInterface::YDLIDAR_TG15: + srs.push_back(20); + break; + case DriverInterface::YDLIDAR_TG30: + srs.push_back(10); + srs.push_back(20); + break; + case DriverInterface::YDLIDAR_TG50: + srs.push_back(20); + break; + case DriverInterface::YDLIDAR_TEA: + srs.push_back(30); + break; + case DriverInterface::YDLIDAR_T15: + srs.push_back(20); + break; + + default: + srs.push_back(4); + break; + } + + return srs; +} + +/*! + * @brief Query whether the LiDAR is Octave LiDAR. + * @param model lidar model + * @return true if the current lidar sampling rate is octave, otherwise false + */ +inline bool isOctaveLidar(int model) +{ + bool ret = false; + if (model == DriverInterface::YDLIDAR_G6 || + model == DriverInterface::YDLIDAR_G7) + { + ret = true; + } + return ret; +} + +//根据雷达码判断是否是Tmini系列雷达 +inline bool isTminiLidar(int model) +{ + return (model == DriverInterface::YDLIDAR_Tmini || + model == DriverInterface::YDLIDAR_TminiPro || + model == DriverInterface::YDLIDAR_TminiPlus || + model == DriverInterface::YDLIDAR_TSAPro); +} + +//根据雷达码判断是否是SCL雷达 +inline bool isSCLLidar2(int model) +{ + return model == DriverInterface::YDLIDAR_SCL; +} + +//根据雷达码判断是否是TEA雷达 +inline bool isTEALidar(int model) +{ + return model == DriverInterface::YDLIDAR_TEA; +} + +/*! + * @brief Supports multiple sampling rate + * @param model lidar model + * @return true if THere are multiple sampling rate, otherwise false. + */ +inline bool hasSampleRate(int model) +{ + bool ret = false; + + if (model == DriverInterface::YDLIDAR_G4 || + model == DriverInterface::YDLIDAR_G5 || + model == DriverInterface::YDLIDAR_G4PRO || + model == DriverInterface::YDLIDAR_F4PRO || + // model == DriverInterface::YDLIDAR_G6 || + model == DriverInterface::YDLIDAR_G7 || + model == DriverInterface::YDLIDAR_TG15 || + model == DriverInterface::YDLIDAR_TG50 || + model == DriverInterface::YDLIDAR_TG30 || + model == DriverInterface::YDLIDAR_TEA) { + ret = true; + } + + return ret; +} + +inline bool isR3Lidar(int model) +{ + if (model == DriverInterface::YDLIDAR_R3) + { + return true; + } + return false; +} + +/*! + * @brief Is there a zero offset angle + * @param model lidar model + * @return true if there are zero offset angle, otherwise false. + */ +inline bool hasZeroAngle(int model) { + bool ret = false; + + if (model == DriverInterface::YDLIDAR_R2 || + model == DriverInterface::YDLIDAR_G2A || + model == DriverInterface::YDLIDAR_G2B || + model == DriverInterface::YDLIDAR_G2C || + model == DriverInterface::YDLIDAR_G1 || + model == DriverInterface::YDLIDAR_TG15 || + model == DriverInterface::YDLIDAR_TG30 || + model == DriverInterface::YDLIDAR_TG50 || + model == DriverInterface::YDLIDAR_TEA) { + ret = true; + } + + return ret; +} + +/*! + * @brief Whether to support adjusting the scanning frequency . + * @param model lidar model + * @return true if supported, otherwise false. + */ +inline bool hasScanFrequencyCtrl(int model) +{ + bool ret = true; + + if (model == DriverInterface::YDLIDAR_S4 || + model == DriverInterface::YDLIDAR_S4B || + model == DriverInterface::YDLIDAR_S2 || + model == DriverInterface::YDLIDAR_X4 || + model == DriverInterface::YDLIDAR_GS1 || + model == DriverInterface::YDLIDAR_GS2 || + model == DriverInterface::YDLIDAR_GS5) + { + ret = false; + } + + return ret; +} + +/*! + * @brief Does SDK support the LiDAR model. + * @param model lidar model + * @return true if supported, otherwise false. + */ +inline bool isSupportLidar(int model) +{ + if (model > DriverInterface::YDLIDAR_None && + model < DriverInterface::YDLIDAR_Tail) + return true; + + return false; +} + +/*! + * @brief Whether to support intensity. + * @param model lidar model + * @return true if supported, otherwise false. + */ +inline bool hasIntensity(int model) +{ + bool ret = false; + + if (model == DriverInterface::YDLIDAR_G2B || + model == DriverInterface::YDLIDAR_G4B || + model == DriverInterface::YDLIDAR_S4B || + model == DriverInterface::YDLIDAR_GS1 || + model == DriverInterface::YDLIDAR_GS2) { + ret = true; + } + + return ret; +} + +/*! + * @brief Whether to support serial DTR enable motor. + * @param model lidar model + * @return true if support serial DTR enable motor, otherwise false. + */ +inline bool isSupportMotorCtrl(int model) { + bool ret = false; + + if (model == DriverInterface::YDLIDAR_X4 || + model == DriverInterface::YDLIDAR_S2 || + model == DriverInterface::YDLIDAR_S4 || + model == DriverInterface::YDLIDAR_S4B) { + ret = true; + + } + + return true; +} + +/*! + * @brief Whether the scanning frequency is supported + * @param model lidar model + * @param frequency scanning frequency + * @return true if supported, otherwise false. + */ +inline bool isSupportScanFrequency(int model, double frequency) +{ + bool ret = false; + + if (model >= DriverInterface::YDLIDAR_TG15) + { + if (1 <= frequency && frequency <= 18) + { + ret = true; + } + + if (model == DriverInterface::YDLIDAR_SDM15) + { + if (10 <= frequency && frequency <= 1800) + ret = true; + } + else if (model >= DriverInterface::YDLIDAR_T15) + { + if (1 <= frequency && frequency <= 50) + { + ret = true; + } + } + else if (model == DriverInterface::YDLIDAR_TEA) + { + if (10 <= frequency && frequency <= 30) + { + ret = true; + } + } + else if (model == DriverInterface::YDLIDAR_Tmini) + { + if (5 <= frequency && frequency <= 12) + { + ret = true; + } + } + } + else + { + if (5 <= frequency && frequency <= 16) + { + ret = true; + } + } + + return ret; +} + +/** + * @brief Whether it is a TOF Model LiDAR + * @param model LiDAR model + * @return tru if it is TOF Model, otherwise false. + */ +inline bool isTOFLidarByModel(int model) { + bool ret = false; + + if (model >= DriverInterface::YDLIDAR_TG15 && + model <= DriverInterface::YDLIDAR_TG50) { + ret = true; + } + + return ret; +} + +/** + * @brief Whether it is a Net TOF Model LiDAR + * @param model LiDAR model + * @return tru if it is Net TOF Model, otherwise false. + */ +inline bool isNetTOFLidarByModel(int model) { + bool ret = false; + + if (model >= DriverInterface::YDLIDAR_T15) { + ret = true; + } + + return ret; +} + +/** + * @brief Whether it is a TOF type LiDAR + * @param type LiDAR type + * @return true if it is a TOF type, otherwise false. + */ +inline bool isTOFLidar(int type) { + bool ret = false; + + if (type == TYPE_TOF) { + ret = true; + } + + return ret; +} + +/** + * @brief Whether it is a network hardware interface TOF type LiDAR + * @param type LiDAR type + * @return true if it is a network hardware interface TOF type, otherwise false. + */ +inline bool isNetTOFLidar(int type) { + bool ret = false; + + if (type == TYPE_TOF_NET) { + ret = true; + } + + return ret; +} + +/** + * @brief Whether it is a Triangle type LiDAR + * @param type LiDAR type + * @return true if it is a Triangle type, otherwise false. + */ +inline bool isTriangleLidar(int type) { + bool ret = false; + + if (type == TYPE_TRIANGLE || + type == TYPE_SCL) { + ret = true; + } + + return ret; +} + +/** + * @brief Whether it is a GS type LiDAR + * @param type LiDAR type + * @return true if it is a Triangle type, otherwise false. + */ +inline bool isGSLidar(int type) +{ + return (type == TYPE_GS); +} + +/** + * @brief Whether it is a SCL type LiDAR + * @param type LiDAR type + * @return true if it is a Triangle SCL type, otherwise false. + */ +inline bool isSCLLidar(int type) +{ + return (type == TYPE_SCL); +} + +inline bool isSDMLidar(int type) +{ + return (type == TYPE_SDM); +} + +inline bool isDTSLidar(int type) +{ + return (type == TYPE_SDM18); +} + +inline bool isTIALidar(int type) +{ + return (type == TYPE_TIA); +} + +/** + * @brief Whether it is Old Version protocol TOF LiDAR + * @param model lidar model + * @param Major firmware Major version + * @param Minor firmware Minor version + * @return true if it is old version protocol, otherwise false. + */ +inline bool isOldVersionTOFLidar(int model, int Major, int Minor) { + bool ret = false; + + if (model == DriverInterface::YDLIDAR_TG15 || + model == DriverInterface::YDLIDAR_TG30 || + model == DriverInterface::YDLIDAR_TG50) { + if (Major <= 1 && Minor <= 2) { + ret = true; + } + + } + + return ret; +} + +inline float lidarZeroOffsetAngleScale(uint8_t model, uint8_t Major, + uint8_t Minor) { + float scale = 4.f; + + if (model == DriverInterface::YDLIDAR_R2) { + scale = 100.f; + + if ((Major == 1 && Minor <= 7) || Major < 1) { + scale = 4.0; + } + } + + return scale; +} + +/*! + * @brief Whether to support Heartbeat. + * @param model lidar model + * @return true if support heartbeat, otherwise false. + */ +inline bool isSupportHeartBeat(int model) { + bool ret = false; + + if (model == DriverInterface::YDLIDAR_G4 || + model == DriverInterface::YDLIDAR_G4PRO) { + ret = true; + } + + return true; +} + +/** + * @brief Whether the sampling rate is valid + * @param smap sampling rate map + * @return true if it is valid, otherwise false. + */ +inline bool isValidSampleRate(std::map smap) +{ + if (smap.size() < 1) { + return false; + } + + if (smap.size() == 1) { + if (smap.begin()->second > 2) { + return true; + } + + return false; + } + + return false; +} + +/** + * @brief convert User sampling rate code to LiDAR sampling code + * @param model LiDAR model + * @param m_SampleRate User sampling rate code + * @param defaultRate LiDAR Defualt sampling rate code + * @return LiDAR sampling rate code + */ +inline int ConvertUserToLidarSmaple(int model, + int sampleRate, + int defaultRate) +{ + int _samp_rate = 9; + switch (sampleRate) + { + case 10: + _samp_rate = DriverInterface::YDLIDAR_RATE_4K; + break; + case 16: + _samp_rate = DriverInterface::YDLIDAR_RATE_8K; + break; + case 18: + _samp_rate = DriverInterface::YDLIDAR_RATE_9K; + break; + case 20: + _samp_rate = DriverInterface::YDLIDAR_RATE_10K; + break; + default: + _samp_rate = defaultRate; + break; + } + + if (!isOctaveLidar(model)) + { + _samp_rate = 2; + switch (sampleRate) + { + case 4: + _samp_rate = DriverInterface::YDLIDAR_RATE_4K; + break; + case 8: + _samp_rate = DriverInterface::YDLIDAR_RATE_8K; + break; + case 9: + _samp_rate = DriverInterface::YDLIDAR_RATE_9K; + break; + default: + break; + } + if (model == DriverInterface::YDLIDAR_F4PRO) + { + _samp_rate = 0; + switch (sampleRate) + { + case 4: + _samp_rate = DriverInterface::YDLIDAR_RATE_4K; + break; + case 6: + _samp_rate = DriverInterface::YDLIDAR_RATE_8K; + break; + default: + break; + } + } + } + + return _samp_rate; +} + +/** + * @brief convert LiDAR sampling rate code to User sampling code + * @param model LiDAR model + * @param rate LiDAR sampling rate code + * @return user sampling code + */ +inline int ConvertLidarToUserSmaple(int model, int rate) +{ + int _samp_rate = 9; + + if (!isOctaveLidar(model) && + !isTOFLidarByModel(model)) + { + switch (rate) + { + case DriverInterface::YDLIDAR_RATE_4K: + _samp_rate = 4; + break; + case DriverInterface::YDLIDAR_RATE_8K: + _samp_rate = 8; + if (model == DriverInterface::YDLIDAR_F4PRO) + _samp_rate = 6; + break; + case DriverInterface::YDLIDAR_RATE_9K: + _samp_rate = 9; + break; + case DriverInterface::YDLIDAR_RATE_10K: + _samp_rate = 10; + break; + default: + //修改默认为当前获取到采样率值 + _samp_rate = rate; + break; + } + } + else + { + switch (rate) + { + case DriverInterface::YDLIDAR_RATE_4K: + _samp_rate = 10; + break; + case DriverInterface::YDLIDAR_RATE_8K: + _samp_rate = 16; + break; + case DriverInterface::YDLIDAR_RATE_9K: + _samp_rate = 18; + break; + case DriverInterface::YDLIDAR_RATE_10K: + _samp_rate = 20; + break; + case 4: + _samp_rate = 10; + break; + default: + //修改默认为当前获取到采样率值 + _samp_rate = rate; + break; + } + } + + return _samp_rate; +} + +/** + * @brief Whether the Value is valid. + * @param value LiDAR CT Byte information + * @return true if it is valid, otherwise false + */ +inline bool isValidValue(uint8_t value) { + if (value & 0x80) { + return false; + } + + return true; +} + +/** + * @brief Whether the Version is valid. + * @param info the LiDAR LaserDebug information + * @return true if it is valid, otherwise false. + */ +inline bool isVersionValid(const LaserDebug &info) { + bool ret = false; + + if (isValidValue(info.cVer) && + isValidValue(info.debug2) && + isValidValue(info.hfVer) && + isValidValue(info.month)) { + ret = true; + } + + return ret; +} + +/** + * @brief Whether the serial number is valid. + * @param info LiDAR LaserDebug information + * @return true if it is valid, otherwise false. + */ +inline bool isSerialNumbValid(const LaserDebug &info) { + bool ret = false; + + if (isValidValue(info.day) && + isValidValue(info.year) && + isValidValue(info.numH) && + isValidValue(info.numH)) { + ret = true; + } + + return ret; +} + +/** + * @brief convert node_info to LaserDebug + * @param node LiDAR node_info information + * @param info LiDAR LaserDebug information + */ +inline void parsePackageNode(const node_info &node, LaserDebug &info) +{ + switch (node.index) { + case 0: + break; + case 1: + info.cVer = node.debugInfo; + break; + case 2: + info.debug2 = node.debugInfo; + break; + case 3: + //健康信息 + info.health = node.debugInfo; + break; + case 4: + info.hfVer = node.debugInfo; + break; + case 5: + info.fVer = node.debugInfo; + break; + case 6: + break; + case 7: + info.model = node.debugInfo; + break; + case 8: + break; + case 9: + info.year = node.debugInfo; + break; + case 10: + info.month = node.debugInfo; + break; + case 11: + info.day = node.debugInfo; + break; + case 12: + info.numH = node.debugInfo; + break; + case 13: + info.numL = node.debugInfo; + break; + default: + break; + } + + // if (info.MaxDebugIndex > node.index) { + // info.W3F4CusMajor_W4F0CusMinor = 0xff; + // } + + if (node.index > info.maxIndex) { + info.maxIndex = node.index; + } +} + +// #include +// #include +/** + * @brief convert LaserDebug information to device_info + * @param info LiDAR LaserDebug information + * @param value LiDAR Device information + * @return true if converted successfully, otherwise false. + */ +inline bool parseLaserDebugInfo(const LaserDebug &debug, device_info &di) +{ + bool ret = false; + + uint8_t model = uint8_t(debug.model & 0x0F); + uint8_t CustomVerMajor = uint8_t(debug.hfVer & 0x0F); + uint8_t CustomVerMinor = debug.fVer; + // uint8_t lidarmodel = uint8_t(debug.debug2) >> 3; + uint8_t hardwareVer = uint8_t(debug.hfVer) >> 4; + + uint8_t Year = uint8_t(debug.year >> 2); + uint8_t Moth = uint8_t(debug.month >> 3); + uint8_t Date = uint8_t(debug.day >> 2); + uint32_t Number = + uint32_t(debug.year & 0x03) << 19 | + uint32_t(debug.month & 0x07) << 16 | + uint32_t(debug.day & 0x03) << 14 | + uint32_t(debug.numH & 0x7F) << 7 | + uint32_t(debug.numL & 0x7F); + + if (Moth && Date && Number) + { + di.firmware_version = uint16_t(CustomVerMajor << 8) | + uint16_t(CustomVerMinor); + di.hardware_version = hardwareVer; + di.model = model; + std::stringstream ss; + ss << std::setw(4) << std::setfill('0') << int(Year + 2020); + ss << std::setw(2) << std::setfill('0') << int(Moth); + ss << std::setw(2) << std::setfill('0') << int(Date); + ss << std::setw(8) << std::setfill('0') << Number; + std::string sn(ss.str()); + // 此处sprintf函数在Python调用中会导致缓存溢出 + // sprintf(reinterpret_cast(di.serialnum), + // "%04u%02u%02u%08u", Year + 2020, Moth, Date, Number); + for (int i = 0; i < SDK_SNLEN && i < sn.size(); i++) + { + di.serialnum[i] = std::stoi(std::string(1, sn.at(i))); + } + + ret = true; + } + + return ret; +} + +YDLIDAR_API inline bool printfDeviceInfo(const device_info &di, + int platformType=EPT_Module) +{ + if (di.firmware_version == 0 && + di.hardware_version == 0) { + return false; + } + + uint8_t Major = (uint8_t)(di.firmware_version >> 8); + uint8_t Minjor = (uint8_t)(di.firmware_version & 0xff); + std::string sn; + for (int i = 0; i < SDK_SNLEN; i++) + sn += char(di.serialnum[i] + 48); //整型值转字符值 + // printf("%01X", di.serialnum[i] & 0xff); + + info("%s device info\n" + "Firmware version: %u.%u\n" + "Hardware version: %u\n" + "Models: %s\n" + "Serial: %s", + EPT_Module == platformType ? "Module" : "Baseplate", + Major, + Minjor, + di.hardware_version, + lidarModelToString(di.model).c_str(), + sn.c_str()); + + return true; +} + +/** + * @brief split string to vector by delim format + * @param s string + * @param delim split format + * @return split vector + */ +inline std::vector split(const std::string &s, char delim) { + std::vector elems; + std::stringstream ss(s); + std::string number; + + while (std::getline(ss, number, delim)) { + elems.push_back(atof(number.c_str())); + } + + return elems; +} + +/** + * @brief Whether the ET LiDAR Protocol type is V1. + * @param protocol LiDAR Protocol Byte information + * @return true if it is V1, otherwise false + */ +inline bool isV1Protocol(uint8_t protocol) +{ + if (protocol == Protocol_V1) { + return true; + } + return false; +} + +//获取数据值（小端序） +inline uint32_t getLittleValue(const uint8_t *data, int size) +{ + uint32_t v = 0; + if (!data || !size) + return v; + for (int i=0; i +#include +#include + +/// 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" diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/CMakeLists.txt new file mode 100644 index 0000000..d38061d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/CMakeLists.txt @@ -0,0 +1,5 @@ +aux_include_directory(. HDRS) +aux_source_directory(. SRCS) +add_to_ydlidar_headers(${HDRS}) +add_to_ydlidar_sources(${SRCS}) + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.c b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.c new file mode 100644 index 0000000..524ba46 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.c @@ -0,0 +1,3119 @@ +/* + 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. +*/ + +/* cJSON */ +/* JSON parser in C. */ + +/* disable warnings about old C89 functions in MSVC */ +#if !defined(_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) +#define _CRT_SECURE_NO_DEPRECATE +#endif + +#ifdef __GNUC__ +#pragma GCC visibility push(default) +#endif +#if defined(_MSC_VER) +#pragma warning (push) +/* disable warning about single line comments in system headers */ +#pragma warning (disable : 4001) +#endif + +#include +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_LOCALES +#include +#endif + +#if defined(_MSC_VER) +#pragma warning (pop) +#endif +#ifdef __GNUC__ +#pragma GCC visibility pop +#endif + +#include "cJSON.h" + +/* define our own boolean type */ +#ifdef true +#undef true +#endif +#define true ((cJSON_bool)1) + +#ifdef false +#undef false +#endif +#define false ((cJSON_bool)0) + +/* define isnan and isinf for ANSI C, if in C99 or above, isnan and isinf has been defined in math.h */ +#ifndef isinf +#define isinf(d) (isnan((d - d)) && !isnan(d)) +#endif +#ifndef isnan +#define isnan(d) (d != d) +#endif + +#ifndef NAN +#ifdef _WIN32 +#define NAN sqrt(-1.0) +#else +#define NAN 0.0/0.0 +#endif +#endif + +typedef struct { + const unsigned char *json; + size_t position; +} error; +static error global_error = { NULL, 0 }; + +CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void) +{ + return (const char*) (global_error.json + global_error.position); +} + +CJSON_PUBLIC(char *) cJSON_GetStringValue(const cJSON * const item) +{ + if (!cJSON_IsString(item)) + { + return NULL; + } + + return item->valuestring; +} + +CJSON_PUBLIC(double) cJSON_GetNumberValue(const cJSON * const item) +{ + if (!cJSON_IsNumber(item)) + { + return (double) NAN; + } + + return item->valuedouble; +} + +/* This is a safeguard to prevent copy-pasters from using incompatible C and header files */ +#if (CJSON_VERSION_MAJOR != 1) || (CJSON_VERSION_MINOR != 7) || (CJSON_VERSION_PATCH != 15) + #error cJSON.h and cJSON.c have different versions. Make sure that both have the same. +#endif + +CJSON_PUBLIC(const char*) cJSON_Version(void) +{ + static char version[15]; + sprintf(version, "%i.%i.%i", CJSON_VERSION_MAJOR, CJSON_VERSION_MINOR, CJSON_VERSION_PATCH); + + return version; +} + +/* Case insensitive string comparison, doesn't consider two NULL pointers equal though */ +static int case_insensitive_strcmp(const unsigned char *string1, const unsigned char *string2) +{ + if ((string1 == NULL) || (string2 == NULL)) + { + return 1; + } + + if (string1 == string2) + { + return 0; + } + + for(; tolower(*string1) == tolower(*string2); (void)string1++, string2++) + { + if (*string1 == '\0') + { + return 0; + } + } + + return tolower(*string1) - tolower(*string2); +} + +typedef struct internal_hooks +{ + void *(CJSON_CDECL *allocate)(size_t size); + void (CJSON_CDECL *deallocate)(void *pointer); + void *(CJSON_CDECL *reallocate)(void *pointer, size_t size); +} internal_hooks; + +#if defined(_MSC_VER) +/* work around MSVC error C2322: '...' address of dllimport '...' is not static */ +static void * CJSON_CDECL internal_malloc(size_t size) +{ + return malloc(size); +} +static void CJSON_CDECL internal_free(void *pointer) +{ + free(pointer); +} +static void * CJSON_CDECL internal_realloc(void *pointer, size_t size) +{ + return realloc(pointer, size); +} +#else +#define internal_malloc malloc +#define internal_free free +#define internal_realloc realloc +#endif + +/* strlen of character literals resolved at compile time */ +#define static_strlen(string_literal) (sizeof(string_literal) - sizeof("")) + +static internal_hooks global_hooks = { internal_malloc, internal_free, internal_realloc }; + +static unsigned char* cJSON_strdup(const unsigned char* string, const internal_hooks * const hooks) +{ + size_t length = 0; + unsigned char *copy = NULL; + + if (string == NULL) + { + return NULL; + } + + length = strlen((const char*)string) + sizeof(""); + copy = (unsigned char*)hooks->allocate(length); + if (copy == NULL) + { + return NULL; + } + memcpy(copy, string, length); + + return copy; +} + +CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks* hooks) +{ + if (hooks == NULL) + { + /* Reset hooks */ + global_hooks.allocate = malloc; + global_hooks.deallocate = free; + global_hooks.reallocate = realloc; + return; + } + + global_hooks.allocate = malloc; + if (hooks->malloc_fn != NULL) + { + global_hooks.allocate = hooks->malloc_fn; + } + + global_hooks.deallocate = free; + if (hooks->free_fn != NULL) + { + global_hooks.deallocate = hooks->free_fn; + } + + /* use realloc only if both free and malloc are used */ + global_hooks.reallocate = NULL; + if ((global_hooks.allocate == malloc) && (global_hooks.deallocate == free)) + { + global_hooks.reallocate = realloc; + } +} + +/* Internal constructor. */ +static cJSON *cJSON_New_Item(const internal_hooks * const hooks) +{ + cJSON* node = (cJSON*)hooks->allocate(sizeof(cJSON)); + if (node) + { + memset(node, '\0', sizeof(cJSON)); + } + + return node; +} + +/* Delete a cJSON structure. */ +CJSON_PUBLIC(void) cJSON_Delete(cJSON *item) +{ + cJSON *next = NULL; + while (item != NULL) + { + next = item->next; + if (!(item->type & cJSON_IsReference) && (item->child != NULL)) + { + cJSON_Delete(item->child); + } + if (!(item->type & cJSON_IsReference) && (item->valuestring != NULL)) + { + global_hooks.deallocate(item->valuestring); + } + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + global_hooks.deallocate(item->string); + } + global_hooks.deallocate(item); + item = next; + } +} + +/* get the decimal point character of the current locale */ +static unsigned char get_decimal_point(void) +{ +#ifdef ENABLE_LOCALES + struct lconv *lconv = localeconv(); + return (unsigned char) lconv->decimal_point[0]; +#else + return '.'; +#endif +} + +typedef struct +{ + const unsigned char *content; + size_t length; + size_t offset; + size_t depth; /* How deeply nested (in arrays/objects) is the input at the current offset. */ + internal_hooks hooks; +} parse_buffer; + +/* check if the given size is left to read in a given parse buffer (starting with 1) */ +#define can_read(buffer, size) ((buffer != NULL) && (((buffer)->offset + size) <= (buffer)->length)) +/* check if the buffer can be accessed at the given index (starting with 0) */ +#define can_access_at_index(buffer, index) ((buffer != NULL) && (((buffer)->offset + index) < (buffer)->length)) +#define cannot_access_at_index(buffer, index) (!can_access_at_index(buffer, index)) +/* get a pointer to the buffer at the position */ +#define buffer_at_offset(buffer) ((buffer)->content + (buffer)->offset) + +/* Parse the input text to generate a number, and populate the result into item. */ +static cJSON_bool parse_number(cJSON * const item, parse_buffer * const input_buffer) +{ + double number = 0; + unsigned char *after_end = NULL; + unsigned char number_c_string[64]; + unsigned char decimal_point = get_decimal_point(); + size_t i = 0; + + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; + } + + /* copy the number into a temporary buffer and replace '.' with the decimal point + * of the current locale (for strtod) + * This also takes care of '\0' not necessarily being available for marking the end of the input */ + for (i = 0; (i < (sizeof(number_c_string) - 1)) && can_access_at_index(input_buffer, i); i++) + { + switch (buffer_at_offset(input_buffer)[i]) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '+': + case '-': + case 'e': + case 'E': + number_c_string[i] = buffer_at_offset(input_buffer)[i]; + break; + + case '.': + number_c_string[i] = decimal_point; + break; + + default: + goto loop_end; + } + } +loop_end: + number_c_string[i] = '\0'; + + number = strtod((const char*)number_c_string, (char**)&after_end); + if (number_c_string == after_end) + { + return false; /* parse_error */ + } + + item->valuedouble = number; + + /* use saturation in case of overflow */ + if (number >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)number; + } + + item->type = cJSON_Number; + + input_buffer->offset += (size_t)(after_end - number_c_string); + return true; +} + +/* don't ask me, but the original cJSON_SetNumberValue returns an integer or double */ +CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number) +{ + if (number >= INT_MAX) + { + object->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + object->valueint = INT_MIN; + } + else + { + object->valueint = (int)number; + } + + return object->valuedouble = number; +} + +CJSON_PUBLIC(char*) cJSON_SetValuestring(cJSON *object, const char *valuestring) +{ + char *copy = NULL; + /* if object's type is not cJSON_String or is cJSON_IsReference, it should not set valuestring */ + if (!(object->type & cJSON_String) || (object->type & cJSON_IsReference)) + { + return NULL; + } + if (strlen(valuestring) <= strlen(object->valuestring)) + { + strcpy(object->valuestring, valuestring); + return object->valuestring; + } + copy = (char*) cJSON_strdup((const unsigned char*)valuestring, &global_hooks); + if (copy == NULL) + { + return NULL; + } + if (object->valuestring != NULL) + { + cJSON_free(object->valuestring); + } + object->valuestring = copy; + + return copy; +} + +typedef struct +{ + unsigned char *buffer; + size_t length; + size_t offset; + size_t depth; /* current nesting depth (for formatted printing) */ + cJSON_bool noalloc; + cJSON_bool format; /* is this print a formatted print */ + internal_hooks hooks; +} printbuffer; + +/* realloc printbuffer if necessary to have at least "needed" bytes more */ +static unsigned char* ensure(printbuffer * const p, size_t needed) +{ + unsigned char *newbuffer = NULL; + size_t newsize = 0; + + if ((p == NULL) || (p->buffer == NULL)) + { + return NULL; + } + + if ((p->length > 0) && (p->offset >= p->length)) + { + /* make sure that offset is valid */ + return NULL; + } + + if (needed > INT_MAX) + { + /* sizes bigger than INT_MAX are currently not supported */ + return NULL; + } + + needed += p->offset + 1; + if (needed <= p->length) + { + return p->buffer + p->offset; + } + + if (p->noalloc) { + return NULL; + } + + /* calculate new buffer size */ + if (needed > (INT_MAX / 2)) + { + /* overflow of int, use INT_MAX if possible */ + if (needed <= INT_MAX) + { + newsize = INT_MAX; + } + else + { + return NULL; + } + } + else + { + newsize = needed * 2; + } + + if (p->hooks.reallocate != NULL) + { + /* reallocate with realloc if available */ + newbuffer = (unsigned char*)p->hooks.reallocate(p->buffer, newsize); + if (newbuffer == NULL) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + } + else + { + /* otherwise reallocate manually */ + newbuffer = (unsigned char*)p->hooks.allocate(newsize); + if (!newbuffer) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + + memcpy(newbuffer, p->buffer, p->offset + 1); + p->hooks.deallocate(p->buffer); + } + p->length = newsize; + p->buffer = newbuffer; + + return newbuffer + p->offset; +} + +/* calculate the new length of the string in a printbuffer and update the offset */ +static void update_offset(printbuffer * const buffer) +{ + const unsigned char *buffer_pointer = NULL; + if ((buffer == NULL) || (buffer->buffer == NULL)) + { + return; + } + buffer_pointer = buffer->buffer + buffer->offset; + + buffer->offset += strlen((const char*)buffer_pointer); +} + +/* securely comparison of floating-point variables */ +static cJSON_bool compare_double(double a, double b) +{ + double maxVal = fabs(a) > fabs(b) ? fabs(a) : fabs(b); + return (fabs(a - b) <= maxVal * DBL_EPSILON); +} + +/* Render the number nicely from the given item into a string. */ +static cJSON_bool print_number(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + double d = item->valuedouble; + int length = 0; + size_t i = 0; + unsigned char number_buffer[26] = {0}; /* temporary buffer to print the number into */ + unsigned char decimal_point = get_decimal_point(); + double test = 0.0; + + if (output_buffer == NULL) + { + return false; + } + + /* This checks for NaN and Infinity */ + if (isnan(d) || isinf(d)) + { + length = sprintf((char*)number_buffer, "null"); + } + else if(d == (double)item->valueint) + { + length = sprintf((char*)number_buffer, "%d", item->valueint); + } + else + { + /* Try 15 decimal places of precision to avoid nonsignificant nonzero digits */ + length = sprintf((char*)number_buffer, "%1.15g", d); + + /* Check whether the original double can be recovered */ + if ((sscanf((char*)number_buffer, "%lg", &test) != 1) || !compare_double((double)test, d)) + { + /* If not, print with 17 decimal places of precision */ + length = sprintf((char*)number_buffer, "%1.17g", d); + } + } + + /* sprintf failed or buffer overrun occurred */ + if ((length < 0) || (length > (int)(sizeof(number_buffer) - 1))) + { + return false; + } + + /* reserve appropriate space in the output */ + output_pointer = ensure(output_buffer, (size_t)length + sizeof("")); + if (output_pointer == NULL) + { + return false; + } + + /* copy the printed number to the output and replace locale + * dependent decimal point with '.' */ + for (i = 0; i < ((size_t)length); i++) + { + if (number_buffer[i] == decimal_point) + { + output_pointer[i] = '.'; + continue; + } + + output_pointer[i] = number_buffer[i]; + } + output_pointer[i] = '\0'; + + output_buffer->offset += (size_t)length; + + return true; +} + +/* parse 4 digit hexadecimal number */ +static unsigned parse_hex4(const unsigned char * const input) +{ + unsigned int h = 0; + size_t i = 0; + + for (i = 0; i < 4; i++) + { + /* parse digit */ + if ((input[i] >= '0') && (input[i] <= '9')) + { + h += (unsigned int) input[i] - '0'; + } + else if ((input[i] >= 'A') && (input[i] <= 'F')) + { + h += (unsigned int) 10 + input[i] - 'A'; + } + else if ((input[i] >= 'a') && (input[i] <= 'f')) + { + h += (unsigned int) 10 + input[i] - 'a'; + } + else /* invalid */ + { + return 0; + } + + if (i < 3) + { + /* shift left to make place for the next nibble */ + h = h << 4; + } + } + + return h; +} + +/* converts a UTF-16 literal to UTF-8 + * A literal can be one or two sequences of the form \uXXXX */ +static unsigned char utf16_literal_to_utf8(const unsigned char * const input_pointer, const unsigned char * const input_end, unsigned char **output_pointer) +{ + long unsigned int codepoint = 0; + unsigned int first_code = 0; + const unsigned char *first_sequence = input_pointer; + unsigned char utf8_length = 0; + unsigned char utf8_position = 0; + unsigned char sequence_length = 0; + unsigned char first_byte_mark = 0; + + if ((input_end - first_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + /* get the first utf16 sequence */ + first_code = parse_hex4(first_sequence + 2); + + /* check that the code is valid */ + if (((first_code >= 0xDC00) && (first_code <= 0xDFFF))) + { + goto fail; + } + + /* UTF16 surrogate pair */ + if ((first_code >= 0xD800) && (first_code <= 0xDBFF)) + { + const unsigned char *second_sequence = first_sequence + 6; + unsigned int second_code = 0; + sequence_length = 12; /* \uXXXX\uXXXX */ + + if ((input_end - second_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + if ((second_sequence[0] != '\\') || (second_sequence[1] != 'u')) + { + /* missing second half of the surrogate pair */ + goto fail; + } + + /* get the second utf16 sequence */ + second_code = parse_hex4(second_sequence + 2); + /* check that the code is valid */ + if ((second_code < 0xDC00) || (second_code > 0xDFFF)) + { + /* invalid second half of the surrogate pair */ + goto fail; + } + + + /* calculate the unicode codepoint from the surrogate pair */ + codepoint = 0x10000 + (((first_code & 0x3FF) << 10) | (second_code & 0x3FF)); + } + else + { + sequence_length = 6; /* \uXXXX */ + codepoint = first_code; + } + + /* encode as UTF-8 + * takes at maximum 4 bytes to encode: + * 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx */ + if (codepoint < 0x80) + { + /* normal ascii, encoding 0xxxxxxx */ + utf8_length = 1; + } + else if (codepoint < 0x800) + { + /* two bytes, encoding 110xxxxx 10xxxxxx */ + utf8_length = 2; + first_byte_mark = 0xC0; /* 11000000 */ + } + else if (codepoint < 0x10000) + { + /* three bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx */ + utf8_length = 3; + first_byte_mark = 0xE0; /* 11100000 */ + } + else if (codepoint <= 0x10FFFF) + { + /* four bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx 10xxxxxx */ + utf8_length = 4; + first_byte_mark = 0xF0; /* 11110000 */ + } + else + { + /* invalid unicode codepoint */ + goto fail; + } + + /* encode as utf8 */ + for (utf8_position = (unsigned char)(utf8_length - 1); utf8_position > 0; utf8_position--) + { + /* 10xxxxxx */ + (*output_pointer)[utf8_position] = (unsigned char)((codepoint | 0x80) & 0xBF); + codepoint >>= 6; + } + /* encode first byte */ + if (utf8_length > 1) + { + (*output_pointer)[0] = (unsigned char)((codepoint | first_byte_mark) & 0xFF); + } + else + { + (*output_pointer)[0] = (unsigned char)(codepoint & 0x7F); + } + + *output_pointer += utf8_length; + + return sequence_length; + +fail: + return 0; +} + +/* Parse the input text into an unescaped cinput, and populate item. */ +static cJSON_bool parse_string(cJSON * const item, parse_buffer * const input_buffer) +{ + const unsigned char *input_pointer = buffer_at_offset(input_buffer) + 1; + const unsigned char *input_end = buffer_at_offset(input_buffer) + 1; + unsigned char *output_pointer = NULL; + unsigned char *output = NULL; + + /* not a string */ + if (buffer_at_offset(input_buffer)[0] != '\"') + { + goto fail; + } + + { + /* calculate approximate size of the output (overestimate) */ + size_t allocation_length = 0; + size_t skipped_bytes = 0; + while (((size_t)(input_end - input_buffer->content) < input_buffer->length) && (*input_end != '\"')) + { + /* is escape sequence */ + if (input_end[0] == '\\') + { + if ((size_t)(input_end + 1 - input_buffer->content) >= input_buffer->length) + { + /* prevent buffer overflow when last input character is a backslash */ + goto fail; + } + skipped_bytes++; + input_end++; + } + input_end++; + } + if (((size_t)(input_end - input_buffer->content) >= input_buffer->length) || (*input_end != '\"')) + { + goto fail; /* string ended unexpectedly */ + } + + /* This is at most how much we need for the output */ + allocation_length = (size_t) (input_end - buffer_at_offset(input_buffer)) - skipped_bytes; + output = (unsigned char*)input_buffer->hooks.allocate(allocation_length + sizeof("")); + if (output == NULL) + { + goto fail; /* allocation failure */ + } + } + + output_pointer = output; + /* loop through the string literal */ + while (input_pointer < input_end) + { + if (*input_pointer != '\\') + { + *output_pointer++ = *input_pointer++; + } + /* escape sequence */ + else + { + unsigned char sequence_length = 2; + if ((input_end - input_pointer) < 1) + { + goto fail; + } + + switch (input_pointer[1]) + { + case 'b': + *output_pointer++ = '\b'; + break; + case 'f': + *output_pointer++ = '\f'; + break; + case 'n': + *output_pointer++ = '\n'; + break; + case 'r': + *output_pointer++ = '\r'; + break; + case 't': + *output_pointer++ = '\t'; + break; + case '\"': + case '\\': + case '/': + *output_pointer++ = input_pointer[1]; + break; + + /* UTF-16 literal */ + case 'u': + sequence_length = utf16_literal_to_utf8(input_pointer, input_end, &output_pointer); + if (sequence_length == 0) + { + /* failed to convert UTF16-literal to UTF-8 */ + goto fail; + } + break; + + default: + goto fail; + } + input_pointer += sequence_length; + } + } + + /* zero terminate the output */ + *output_pointer = '\0'; + + item->type = cJSON_String; + item->valuestring = (char*)output; + + input_buffer->offset = (size_t) (input_end - input_buffer->content); + input_buffer->offset++; + + return true; + +fail: + if (output != NULL) + { + input_buffer->hooks.deallocate(output); + } + + if (input_pointer != NULL) + { + input_buffer->offset = (size_t)(input_pointer - input_buffer->content); + } + + return false; +} + +/* Render the cstring provided to an escaped version that can be printed. */ +static cJSON_bool print_string_ptr(const unsigned char * const input, printbuffer * const output_buffer) +{ + const unsigned char *input_pointer = NULL; + unsigned char *output = NULL; + unsigned char *output_pointer = NULL; + size_t output_length = 0; + /* numbers of additional characters needed for escaping */ + size_t escape_characters = 0; + + if (output_buffer == NULL) + { + return false; + } + + /* empty string */ + if (input == NULL) + { + output = ensure(output_buffer, sizeof("\"\"")); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "\"\""); + + return true; + } + + /* set "flag" to 1 if something needs to be escaped */ + for (input_pointer = input; *input_pointer; input_pointer++) + { + switch (*input_pointer) + { + case '\"': + case '\\': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + /* one character escape sequence */ + escape_characters++; + break; + default: + if (*input_pointer < 32) + { + /* UTF-16 escape sequence uXXXX */ + escape_characters += 5; + } + break; + } + } + output_length = (size_t)(input_pointer - input) + escape_characters; + + output = ensure(output_buffer, output_length + sizeof("\"\"")); + if (output == NULL) + { + return false; + } + + /* no characters have to be escaped */ + if (escape_characters == 0) + { + output[0] = '\"'; + memcpy(output + 1, input, output_length); + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; + } + + output[0] = '\"'; + output_pointer = output + 1; + /* copy the string */ + for (input_pointer = input; *input_pointer != '\0'; (void)input_pointer++, output_pointer++) + { + if ((*input_pointer > 31) && (*input_pointer != '\"') && (*input_pointer != '\\')) + { + /* normal character, copy */ + *output_pointer = *input_pointer; + } + else + { + /* character needs to be escaped */ + *output_pointer++ = '\\'; + switch (*input_pointer) + { + case '\\': + *output_pointer = '\\'; + break; + case '\"': + *output_pointer = '\"'; + break; + case '\b': + *output_pointer = 'b'; + break; + case '\f': + *output_pointer = 'f'; + break; + case '\n': + *output_pointer = 'n'; + break; + case '\r': + *output_pointer = 'r'; + break; + case '\t': + *output_pointer = 't'; + break; + default: + /* escape and print as unicode codepoint */ + sprintf((char*)output_pointer, "u%04x", *input_pointer); + output_pointer += 4; + break; + } + } + } + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; +} + +/* Invoke print_string_ptr (which is useful) on an item. */ +static cJSON_bool print_string(const cJSON * const item, printbuffer * const p) +{ + return print_string_ptr((unsigned char*)item->valuestring, p); +} + +/* Predeclare these prototypes. */ +static cJSON_bool parse_value(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_value(const cJSON * const item, printbuffer * const output_buffer); +static cJSON_bool parse_array(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_array(const cJSON * const item, printbuffer * const output_buffer); +static cJSON_bool parse_object(cJSON * const item, parse_buffer * const input_buffer); +static cJSON_bool print_object(const cJSON * const item, printbuffer * const output_buffer); + +/* Utility to jump whitespace and cr/lf */ +static parse_buffer *buffer_skip_whitespace(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL)) + { + return NULL; + } + + if (cannot_access_at_index(buffer, 0)) + { + return buffer; + } + + while (can_access_at_index(buffer, 0) && (buffer_at_offset(buffer)[0] <= 32)) + { + buffer->offset++; + } + + if (buffer->offset == buffer->length) + { + buffer->offset--; + } + + return buffer; +} + +/* skip the UTF-8 BOM (byte order mark) if it is at the beginning of a buffer */ +static parse_buffer *skip_utf8_bom(parse_buffer * const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL) || (buffer->offset != 0)) + { + return NULL; + } + + if (can_access_at_index(buffer, 4) && (strncmp((const char*)buffer_at_offset(buffer), "\xEF\xBB\xBF", 3) == 0)) + { + buffer->offset += 3; + } + + return buffer; +} + +CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + size_t buffer_length; + + if (NULL == value) + { + return NULL; + } + + /* Adding null character size due to require_null_terminated. */ + buffer_length = strlen(value) + sizeof(""); + + return cJSON_ParseWithLengthOpts(value, buffer_length, return_parse_end, require_null_terminated); +} + +/* Parse an object - create a new root, and populate. */ +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + parse_buffer buffer = { 0, 0, 0, 0, { 0, 0, 0 } }; + cJSON *item = NULL; + + /* reset error position */ + global_error.json = NULL; + global_error.position = 0; + + if (value == NULL || 0 == buffer_length) + { + goto fail; + } + + buffer.content = (const unsigned char*)value; + buffer.length = buffer_length; + buffer.offset = 0; + buffer.hooks = global_hooks; + + item = cJSON_New_Item(&global_hooks); + if (item == NULL) /* memory fail */ + { + goto fail; + } + + if (!parse_value(item, buffer_skip_whitespace(skip_utf8_bom(&buffer)))) + { + /* parse failure. ep is set. */ + goto fail; + } + + /* if we require null-terminated JSON without appended garbage, skip and then check for a null terminator */ + if (require_null_terminated) + { + buffer_skip_whitespace(&buffer); + if ((buffer.offset >= buffer.length) || buffer_at_offset(&buffer)[0] != '\0') + { + goto fail; + } + } + if (return_parse_end) + { + *return_parse_end = (const char*)buffer_at_offset(&buffer); + } + + return item; + +fail: + if (item != NULL) + { + cJSON_Delete(item); + } + + if (value != NULL) + { + error local_error; + local_error.json = (const unsigned char*)value; + local_error.position = 0; + + if (buffer.offset < buffer.length) + { + local_error.position = buffer.offset; + } + else if (buffer.length > 0) + { + local_error.position = buffer.length - 1; + } + + if (return_parse_end != NULL) + { + *return_parse_end = (const char*)local_error.json + local_error.position; + } + + global_error = local_error; + } + + return NULL; +} + +/* Default options for cJSON_Parse */ +CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value) +{ + return cJSON_ParseWithOpts(value, 0, 0); +} + +CJSON_PUBLIC(cJSON *) cJSON_ParseWithLength(const char *value, size_t buffer_length) +{ + return cJSON_ParseWithLengthOpts(value, buffer_length, 0, 0); +} + +#define cjson_min(a, b) (((a) < (b)) ? (a) : (b)) + +static unsigned char *print(const cJSON * const item, cJSON_bool format, const internal_hooks * const hooks) +{ + static const size_t default_buffer_size = 256; + printbuffer buffer[1]; + unsigned char *printed = NULL; + + memset(buffer, 0, sizeof(buffer)); + + /* create buffer */ + buffer->buffer = (unsigned char*) hooks->allocate(default_buffer_size); + buffer->length = default_buffer_size; + buffer->format = format; + buffer->hooks = *hooks; + if (buffer->buffer == NULL) + { + goto fail; + } + + /* print the value */ + if (!print_value(item, buffer)) + { + goto fail; + } + update_offset(buffer); + + /* check if reallocate is available */ + if (hooks->reallocate != NULL) + { + printed = (unsigned char*) hooks->reallocate(buffer->buffer, buffer->offset + 1); + if (printed == NULL) { + goto fail; + } + buffer->buffer = NULL; + } + else /* otherwise copy the JSON over to a new buffer */ + { + printed = (unsigned char*) hooks->allocate(buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + memcpy(printed, buffer->buffer, cjson_min(buffer->length, buffer->offset + 1)); + printed[buffer->offset] = '\0'; /* just to be sure */ + + /* free the buffer */ + hooks->deallocate(buffer->buffer); + } + + return printed; + +fail: + if (buffer->buffer != NULL) + { + hooks->deallocate(buffer->buffer); + } + + if (printed != NULL) + { + hooks->deallocate(printed); + } + + return NULL; +} + +/* Render a cJSON item/entity/structure to text. */ +CJSON_PUBLIC(char *) cJSON_Print(const cJSON *item) +{ + return (char*)print(item, true, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON *item) +{ + return (char*)print(item, false, &global_hooks); +} + +CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if (prebuffer < 0) + { + return NULL; + } + + p.buffer = (unsigned char*)global_hooks.allocate((size_t)prebuffer); + if (!p.buffer) + { + return NULL; + } + + p.length = (size_t)prebuffer; + p.offset = 0; + p.noalloc = false; + p.format = fmt; + p.hooks = global_hooks; + + if (!print_value(item, &p)) + { + global_hooks.deallocate(p.buffer); + return NULL; + } + + return (char*)p.buffer; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format) +{ + printbuffer p = { 0, 0, 0, 0, 0, 0, { 0, 0, 0 } }; + + if ((length < 0) || (buffer == NULL)) + { + return false; + } + + p.buffer = (unsigned char*)buffer; + p.length = (size_t)length; + p.offset = 0; + p.noalloc = true; + p.format = format; + p.hooks = global_hooks; + + return print_value(item, &p); +} + +/* Parser core - when encountering text, process appropriately. */ +static cJSON_bool parse_value(cJSON * const item, parse_buffer * const input_buffer) +{ + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; /* no input */ + } + + /* parse the different types of values */ + /* null */ + if (can_read(input_buffer, 4) && (strncmp((const char*)buffer_at_offset(input_buffer), "null", 4) == 0)) + { + item->type = cJSON_NULL; + input_buffer->offset += 4; + return true; + } + /* false */ + if (can_read(input_buffer, 5) && (strncmp((const char*)buffer_at_offset(input_buffer), "false", 5) == 0)) + { + item->type = cJSON_False; + input_buffer->offset += 5; + return true; + } + /* true */ + if (can_read(input_buffer, 4) && (strncmp((const char*)buffer_at_offset(input_buffer), "true", 4) == 0)) + { + item->type = cJSON_True; + item->valueint = 1; + input_buffer->offset += 4; + return true; + } + /* string */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '\"')) + { + return parse_string(item, input_buffer); + } + /* number */ + if (can_access_at_index(input_buffer, 0) && ((buffer_at_offset(input_buffer)[0] == '-') || ((buffer_at_offset(input_buffer)[0] >= '0') && (buffer_at_offset(input_buffer)[0] <= '9')))) + { + return parse_number(item, input_buffer); + } + /* array */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '[')) + { + return parse_array(item, input_buffer); + } + /* object */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '{')) + { + return parse_object(item, input_buffer); + } + + return false; +} + +/* Render a value to text. */ +static cJSON_bool print_value(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output = NULL; + + if ((item == NULL) || (output_buffer == NULL)) + { + return false; + } + + switch ((item->type) & 0xFF) + { + case cJSON_NULL: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "null"); + return true; + + case cJSON_False: + output = ensure(output_buffer, 6); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "false"); + return true; + + case cJSON_True: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char*)output, "true"); + return true; + + case cJSON_Number: + return print_number(item, output_buffer); + + case cJSON_Raw: + { + size_t raw_length = 0; + if (item->valuestring == NULL) + { + return false; + } + + raw_length = strlen(item->valuestring) + sizeof(""); + output = ensure(output_buffer, raw_length); + if (output == NULL) + { + return false; + } + memcpy(output, item->valuestring, raw_length); + return true; + } + + case cJSON_String: + return print_string(item, output_buffer); + + case cJSON_Array: + return print_array(item, output_buffer); + + case cJSON_Object: + return print_object(item, output_buffer); + + default: + return false; + } +} + +/* Build an array from input text. */ +static cJSON_bool parse_array(cJSON * const item, parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* head of the linked list */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (buffer_at_offset(input_buffer)[0] != '[') + { + /* not an array */ + goto fail; + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ']')) + { + /* empty array */ + goto success; + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse next value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || buffer_at_offset(input_buffer)[0] != ']') + { + goto fail; /* expected end of array */ + } + +success: + input_buffer->depth--; + + if (head != NULL) { + head->prev = current_item; + } + + item->type = cJSON_Array; + item->child = head; + + input_buffer->offset++; + + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an array to text */ +static cJSON_bool print_array(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_element = item->child; + + if (output_buffer == NULL) + { + return false; + } + + /* Compose the output array. */ + /* opening square bracket */ + output_pointer = ensure(output_buffer, 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer = '['; + output_buffer->offset++; + output_buffer->depth++; + + while (current_element != NULL) + { + if (!print_value(current_element, output_buffer)) + { + return false; + } + update_offset(output_buffer); + if (current_element->next) + { + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ','; + if(output_buffer->format) + { + *output_pointer++ = ' '; + } + *output_pointer = '\0'; + output_buffer->offset += length; + } + current_element = current_element->next; + } + + output_pointer = ensure(output_buffer, 2); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ']'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Build an object from the text. */ +static cJSON_bool parse_object(cJSON * const item, parse_buffer * const input_buffer) +{ + cJSON *head = NULL; /* linked list head */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '{')) + { + goto fail; /* not an object */ + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '}')) + { + goto success; /* empty object */ + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse the name of the child */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_string(current_item, input_buffer)) + { + goto fail; /* failed to parse name */ + } + buffer_skip_whitespace(input_buffer); + + /* swap valuestring and string, because we parsed the name */ + current_item->string = current_item->valuestring; + current_item->valuestring = NULL; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != ':')) + { + goto fail; /* invalid object */ + } + + /* parse the value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } + while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '}')) + { + goto fail; /* expected end of object */ + } + +success: + input_buffer->depth--; + + if (head != NULL) { + head->prev = current_item; + } + + item->type = cJSON_Object; + item->child = head; + + input_buffer->offset++; + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an object to text. */ +static cJSON_bool print_object(const cJSON * const item, printbuffer * const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_item = item->child; + + if (output_buffer == NULL) + { + return false; + } + + /* Compose the output: */ + length = (size_t) (output_buffer->format ? 2 : 1); /* fmt: {\n */ + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer++ = '{'; + output_buffer->depth++; + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + output_buffer->offset += length; + + while (current_item) + { + if (output_buffer->format) + { + size_t i; + output_pointer = ensure(output_buffer, output_buffer->depth); + if (output_pointer == NULL) + { + return false; + } + for (i = 0; i < output_buffer->depth; i++) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += output_buffer->depth; + } + + /* print key */ + if (!print_string_ptr((unsigned char*)current_item->string, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + length = (size_t) (output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ':'; + if (output_buffer->format) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += length; + + /* print value */ + if (!print_value(current_item, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + /* print comma if not last */ + length = ((size_t)(output_buffer->format ? 1 : 0) + (size_t)(current_item->next ? 1 : 0)); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + if (current_item->next) + { + *output_pointer++ = ','; + } + + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + *output_pointer = '\0'; + output_buffer->offset += length; + + current_item = current_item->next; + } + + output_pointer = ensure(output_buffer, output_buffer->format ? (output_buffer->depth + 1) : 2); + if (output_pointer == NULL) + { + return false; + } + if (output_buffer->format) + { + size_t i; + for (i = 0; i < (output_buffer->depth - 1); i++) + { + *output_pointer++ = '\t'; + } + } + *output_pointer++ = '}'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Get Array size/item / object item. */ +CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array) +{ + cJSON *child = NULL; + size_t size = 0; + + if (array == NULL) + { + return 0; + } + + child = array->child; + + while(child != NULL) + { + size++; + child = child->next; + } + + /* FIXME: Can overflow here. Cannot be fixed without breaking the API */ + + return (int)size; +} + +static cJSON* get_array_item(const cJSON *array, size_t index) +{ + cJSON *current_child = NULL; + + if (array == NULL) + { + return NULL; + } + + current_child = array->child; + while ((current_child != NULL) && (index > 0)) + { + index--; + current_child = current_child->next; + } + + return current_child; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON *array, int index) +{ + if (index < 0) + { + return NULL; + } + + return get_array_item(array, (size_t)index); +} + +static cJSON *get_object_item(const cJSON * const object, const char * const name, const cJSON_bool case_sensitive) +{ + cJSON *current_element = NULL; + + if ((object == NULL) || (name == NULL)) + { + return NULL; + } + + current_element = object->child; + if (case_sensitive) + { + while ((current_element != NULL) && (current_element->string != NULL) && (strcmp(name, current_element->string) != 0)) + { + current_element = current_element->next; + } + } + else + { + while ((current_element != NULL) && (case_insensitive_strcmp((const unsigned char*)name, (const unsigned char*)(current_element->string)) != 0)) + { + current_element = current_element->next; + } + } + + if ((current_element == NULL) || (current_element->string == NULL)) { + return NULL; + } + + return current_element; +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, const char * const string) +{ + return get_object_item(object, string, false); +} + +CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive(const cJSON * const object, const char * const string) +{ + return get_object_item(object, string, true); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string) +{ + return cJSON_GetObjectItem(object, string) ? 1 : 0; +} + +/* Utility for array list handling. */ +static void suffix_object(cJSON *prev, cJSON *item) +{ + prev->next = item; + item->prev = prev; +} + +/* Utility for handling references. */ +static cJSON *create_reference(const cJSON *item, const internal_hooks * const hooks) +{ + cJSON *reference = NULL; + if (item == NULL) + { + return NULL; + } + + reference = cJSON_New_Item(hooks); + if (reference == NULL) + { + return NULL; + } + + memcpy(reference, item, sizeof(cJSON)); + reference->string = NULL; + reference->type |= cJSON_IsReference; + reference->next = reference->prev = NULL; + return reference; +} + +static cJSON_bool add_item_to_array(cJSON *array, cJSON *item) +{ + cJSON *child = NULL; + + if ((item == NULL) || (array == NULL) || (array == item)) + { + return false; + } + + child = array->child; + /* + * To find the last item in array quickly, we use prev in array + */ + if (child == NULL) + { + /* list is empty, start new one */ + array->child = item; + item->prev = item; + item->next = NULL; + } + else + { + /* append to the end */ + if (child->prev) + { + suffix_object(child->prev, item); + array->child->prev = item; + } + } + + return true; +} + +/* Add item to array/object. */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToArray(cJSON *array, cJSON *item) +{ + return add_item_to_array(array, item); +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) + #pragma GCC diagnostic push +#endif +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif +/* helper function to cast away const */ +static void* cast_away_const(const void* string) +{ + return (void*)string; +} +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) + #pragma GCC diagnostic pop +#endif + + +static cJSON_bool add_item_to_object(cJSON * const object, const char * const string, cJSON * const item, const internal_hooks * const hooks, const cJSON_bool constant_key) +{ + char *new_key = NULL; + int new_type = cJSON_Invalid; + + if ((object == NULL) || (string == NULL) || (item == NULL) || (object == item)) + { + return false; + } + + if (constant_key) + { + new_key = (char*)cast_away_const(string); + new_type = item->type | cJSON_StringIsConst; + } + else + { + new_key = (char*)cJSON_strdup((const unsigned char*)string, hooks); + if (new_key == NULL) + { + return false; + } + + new_type = item->type & ~cJSON_StringIsConst; + } + + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + hooks->deallocate(item->string); + } + + item->string = new_key; + item->type = new_type; + + return add_item_to_array(object, item); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, false); +} + +/* Add an item to an object with constant string as key */ +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, true); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item) +{ + if (array == NULL) + { + return false; + } + + return add_item_to_array(array, create_reference(item, &global_hooks)); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item) +{ + if ((object == NULL) || (string == NULL)) + { + return false; + } + + return add_item_to_object(object, string, create_reference(item, &global_hooks), &global_hooks, false); +} + +CJSON_PUBLIC(cJSON*) cJSON_AddNullToObject(cJSON * const object, const char * const name) +{ + cJSON *null = cJSON_CreateNull(); + if (add_item_to_object(object, name, null, &global_hooks, false)) + { + return null; + } + + cJSON_Delete(null); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddTrueToObject(cJSON * const object, const char * const name) +{ + cJSON *true_item = cJSON_CreateTrue(); + if (add_item_to_object(object, name, true_item, &global_hooks, false)) + { + return true_item; + } + + cJSON_Delete(true_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddFalseToObject(cJSON * const object, const char * const name) +{ + cJSON *false_item = cJSON_CreateFalse(); + if (add_item_to_object(object, name, false_item, &global_hooks, false)) + { + return false_item; + } + + cJSON_Delete(false_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddBoolToObject(cJSON * const object, const char * const name, const cJSON_bool boolean) +{ + cJSON *bool_item = cJSON_CreateBool(boolean); + if (add_item_to_object(object, name, bool_item, &global_hooks, false)) + { + return bool_item; + } + + cJSON_Delete(bool_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddNumberToObject(cJSON * const object, const char * const name, const double number) +{ + cJSON *number_item = cJSON_CreateNumber(number); + if (add_item_to_object(object, name, number_item, &global_hooks, false)) + { + return number_item; + } + + cJSON_Delete(number_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddStringToObject(cJSON * const object, const char * const name, const char * const string) +{ + cJSON *string_item = cJSON_CreateString(string); + if (add_item_to_object(object, name, string_item, &global_hooks, false)) + { + return string_item; + } + + cJSON_Delete(string_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddRawToObject(cJSON * const object, const char * const name, const char * const raw) +{ + cJSON *raw_item = cJSON_CreateRaw(raw); + if (add_item_to_object(object, name, raw_item, &global_hooks, false)) + { + return raw_item; + } + + cJSON_Delete(raw_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddObjectToObject(cJSON * const object, const char * const name) +{ + cJSON *object_item = cJSON_CreateObject(); + if (add_item_to_object(object, name, object_item, &global_hooks, false)) + { + return object_item; + } + + cJSON_Delete(object_item); + return NULL; +} + +CJSON_PUBLIC(cJSON*) cJSON_AddArrayToObject(cJSON * const object, const char * const name) +{ + cJSON *array = cJSON_CreateArray(); + if (add_item_to_object(object, name, array, &global_hooks, false)) + { + return array; + } + + cJSON_Delete(array); + return NULL; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON *parent, cJSON * const item) +{ + if ((parent == NULL) || (item == NULL)) + { + return NULL; + } + + if (item != parent->child) + { + /* not the first element */ + item->prev->next = item->next; + } + if (item->next != NULL) + { + /* not the last element */ + item->next->prev = item->prev; + } + + if (item == parent->child) + { + /* first element */ + parent->child = item->next; + } + else if (item->next == NULL) + { + /* last element */ + parent->child->prev = item->prev; + } + + /* make sure the detached item doesn't point anywhere anymore */ + item->prev = NULL; + item->next = NULL; + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON *array, int which) +{ + if (which < 0) + { + return NULL; + } + + return cJSON_DetachItemViaPointer(array, get_array_item(array, (size_t)which)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which) +{ + cJSON_Delete(cJSON_DetachItemFromArray(array, which)); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItem(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItemCaseSensitive(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObject(object, string)); +} + +CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObjectCaseSensitive(object, string)); +} + +/* Replace array/object items with new ones. */ +CJSON_PUBLIC(cJSON_bool) cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem) +{ + cJSON *after_inserted = NULL; + + if (which < 0) + { + return false; + } + + after_inserted = get_array_item(array, (size_t)which); + if (after_inserted == NULL) + { + return add_item_to_array(array, newitem); + } + + newitem->next = after_inserted; + newitem->prev = after_inserted->prev; + after_inserted->prev = newitem; + if (after_inserted == array->child) + { + array->child = newitem; + } + else + { + newitem->prev->next = newitem; + } + return true; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, cJSON * const item, cJSON * replacement) +{ + if ((parent == NULL) || (replacement == NULL) || (item == NULL)) + { + return false; + } + + if (replacement == item) + { + return true; + } + + replacement->next = item->next; + replacement->prev = item->prev; + + if (replacement->next != NULL) + { + replacement->next->prev = replacement; + } + if (parent->child == item) + { + if (parent->child->prev == parent->child) + { + replacement->prev = replacement; + } + parent->child = replacement; + } + else + { /* + * To find the last item in array quickly, we use prev in array. + * We can't modify the last item's next pointer where this item was the parent's child + */ + if (replacement->prev != NULL) + { + replacement->prev->next = replacement; + } + if (replacement->next == NULL) + { + parent->child->prev = replacement; + } + } + + item->next = NULL; + item->prev = NULL; + cJSON_Delete(item); + + return true; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem) +{ + if (which < 0) + { + return false; + } + + return cJSON_ReplaceItemViaPointer(array, get_array_item(array, (size_t)which), newitem); +} + +static cJSON_bool replace_item_in_object(cJSON *object, const char *string, cJSON *replacement, cJSON_bool case_sensitive) +{ + if ((replacement == NULL) || (string == NULL)) + { + return false; + } + + /* replace the name in the replacement */ + if (!(replacement->type & cJSON_StringIsConst) && (replacement->string != NULL)) + { + cJSON_free(replacement->string); + } + replacement->string = (char*)cJSON_strdup((const unsigned char*)string, &global_hooks); + if (replacement->string == NULL) + { + return false; + } + + replacement->type &= ~cJSON_StringIsConst; + + return cJSON_ReplaceItemViaPointer(object, get_object_item(object, string, case_sensitive), replacement); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, false); +} + +CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, true); +} + +/* Create basic types: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_NULL; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_True; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = boolean ? cJSON_True : cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_Number; + item->valuedouble = num; + + /* use saturation in case of overflow */ + if (num >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (num <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)num; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_String; + item->valuestring = (char*)cJSON_strdup((const unsigned char*)string, &global_hooks); + if(!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_String | cJSON_IsReference; + item->valuestring = (char*)cast_away_const(string); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Object | cJSON_IsReference; + item->child = (cJSON*)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON *child) { + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) { + item->type = cJSON_Array | cJSON_IsReference; + item->child = (cJSON*)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type = cJSON_Raw; + item->valuestring = (char*)cJSON_strdup((const unsigned char*)raw, &global_hooks); + if(!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if(item) + { + item->type=cJSON_Array; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Object; + } + + return item; +} + +/* Create Arrays: */ +CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber((double)numbers[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for(i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char *const *strings, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (strings == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateString(strings[i]); + if(!n) + { + cJSON_Delete(a); + return NULL; + } + if(!i) + { + a->child = n; + } + else + { + suffix_object(p,n); + } + p = n; + } + + if (a && a->child) { + a->child->prev = n; + } + + return a; +} + +/* Duplication */ +CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON *item, cJSON_bool recurse) +{ + cJSON *newitem = NULL; + cJSON *child = NULL; + cJSON *next = NULL; + cJSON *newchild = NULL; + + /* Bail on bad ptr */ + if (!item) + { + goto fail; + } + /* Create new item */ + newitem = cJSON_New_Item(&global_hooks); + if (!newitem) + { + goto fail; + } + /* Copy over all vars */ + newitem->type = item->type & (~cJSON_IsReference); + newitem->valueint = item->valueint; + newitem->valuedouble = item->valuedouble; + if (item->valuestring) + { + newitem->valuestring = (char*)cJSON_strdup((unsigned char*)item->valuestring, &global_hooks); + if (!newitem->valuestring) + { + goto fail; + } + } + if (item->string) + { + newitem->string = (item->type&cJSON_StringIsConst) ? item->string : (char*)cJSON_strdup((unsigned char*)item->string, &global_hooks); + if (!newitem->string) + { + goto fail; + } + } + /* If non-recursive, then we're done! */ + if (!recurse) + { + return newitem; + } + /* Walk the ->next chain for the child. */ + child = item->child; + while (child != NULL) + { + newchild = cJSON_Duplicate(child, true); /* Duplicate (with recurse) each item in the ->next chain */ + if (!newchild) + { + goto fail; + } + if (next != NULL) + { + /* If newitem->child already set, then crosswire ->prev and ->next and move on */ + next->next = newchild; + newchild->prev = next; + next = newchild; + } + else + { + /* Set newitem->child and move to it */ + newitem->child = newchild; + next = newchild; + } + child = child->next; + } + if (newitem && newitem->child) + { + newitem->child->prev = newchild; + } + + return newitem; + +fail: + if (newitem != NULL) + { + cJSON_Delete(newitem); + } + + return NULL; +} + +static void skip_oneline_comment(char **input) +{ + *input += static_strlen("//"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if ((*input)[0] == '\n') { + *input += static_strlen("\n"); + return; + } + } +} + +static void skip_multiline_comment(char **input) +{ + *input += static_strlen("/*"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if (((*input)[0] == '*') && ((*input)[1] == '/')) + { + *input += static_strlen("*/"); + return; + } + } +} + +static void minify_string(char **input, char **output) { + (*output)[0] = (*input)[0]; + *input += static_strlen("\""); + *output += static_strlen("\""); + + + for (; (*input)[0] != '\0'; (void)++(*input), ++(*output)) { + (*output)[0] = (*input)[0]; + + if ((*input)[0] == '\"') { + (*output)[0] = '\"'; + *input += static_strlen("\""); + *output += static_strlen("\""); + return; + } else if (((*input)[0] == '\\') && ((*input)[1] == '\"')) { + (*output)[1] = (*input)[1]; + *input += static_strlen("\""); + *output += static_strlen("\""); + } + } +} + +CJSON_PUBLIC(void) cJSON_Minify(char *json) +{ + char *into = json; + + if (json == NULL) + { + return; + } + + while (json[0] != '\0') + { + switch (json[0]) + { + case ' ': + case '\t': + case '\r': + case '\n': + json++; + break; + + case '/': + if (json[1] == '/') + { + skip_oneline_comment(&json); + } + else if (json[1] == '*') + { + skip_multiline_comment(&json); + } else { + json++; + } + break; + + case '\"': + minify_string(&json, (char**)&into); + break; + + default: + into[0] = json[0]; + json++; + into++; + } + } + + /* and null-terminate. */ + *into = '\0'; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Invalid; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_False; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xff) == cJSON_True; +} + + +CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & (cJSON_True | cJSON_False)) != 0; +} +CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_NULL; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Number; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_String; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Array; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Object; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Raw; +} + +CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, const cJSON * const b, const cJSON_bool case_sensitive) +{ + if ((a == NULL) || (b == NULL) || ((a->type & 0xFF) != (b->type & 0xFF))) + { + return false; + } + + /* check if type is valid */ + switch (a->type & 0xFF) + { + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + case cJSON_Number: + case cJSON_String: + case cJSON_Raw: + case cJSON_Array: + case cJSON_Object: + break; + + default: + return false; + } + + /* identical objects are equal */ + if (a == b) + { + return true; + } + + switch (a->type & 0xFF) + { + /* in these cases and equal type is enough */ + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + return true; + + case cJSON_Number: + if (compare_double(a->valuedouble, b->valuedouble)) + { + return true; + } + return false; + + case cJSON_String: + case cJSON_Raw: + if ((a->valuestring == NULL) || (b->valuestring == NULL)) + { + return false; + } + if (strcmp(a->valuestring, b->valuestring) == 0) + { + return true; + } + + return false; + + case cJSON_Array: + { + cJSON *a_element = a->child; + cJSON *b_element = b->child; + + for (; (a_element != NULL) && (b_element != NULL);) + { + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + + a_element = a_element->next; + b_element = b_element->next; + } + + /* one of the arrays is longer than the other */ + if (a_element != b_element) { + return false; + } + + return true; + } + + case cJSON_Object: + { + cJSON *a_element = NULL; + cJSON *b_element = NULL; + cJSON_ArrayForEach(a_element, a) + { + /* TODO This has O(n^2) runtime, which is horrible! */ + b_element = get_object_item(b, a_element->string, case_sensitive); + if (b_element == NULL) + { + return false; + } + + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + } + + /* doing this twice, once on a and b to prevent true comparison if a subset of b + * TODO: Do this the proper way, this is just a fix for now */ + cJSON_ArrayForEach(b_element, b) + { + a_element = get_object_item(a, b_element->string, case_sensitive); + if (a_element == NULL) + { + return false; + } + + if (!cJSON_Compare(b_element, a_element, case_sensitive)) + { + return false; + } + } + + return true; + } + + default: + return false; + } +} + +CJSON_PUBLIC(void *) cJSON_malloc(size_t size) +{ + return global_hooks.allocate(size); +} + +CJSON_PUBLIC(void) cJSON_free(void *object) +{ + global_hooks.deallocate(object); +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.h new file mode 100644 index 0000000..0669975 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/json/cJSON.h @@ -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 + +/* 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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/CMakeLists.txt new file mode 100644 index 0000000..d38061d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/CMakeLists.txt @@ -0,0 +1,5 @@ +aux_include_directory(. HDRS) +aux_source_directory(. SRCS) +add_to_ydlidar_headers(${HDRS}) +add_to_ydlidar_sources(${SRCS}) + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/angles.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/angles.h new file mode 100644 index 0000000..3e6ae7d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/math/angles.h @@ -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 +#include + +#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(delta[1], delta[3]); + return true; + } + + if (fabs(delta[1]) < 1e-6) { + result_max_delta = delta[1]; + result_min_delta = std::min(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(delta, delta_mod_2pi); + } else if (fabs(min_delta_to) > fabs(max_delta_to)) { + shortest_angle = std::min(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(delta, delta_mod_2pi); + } else if (fabs(min_delta) > fabs(max_delta)) { + shortest_angle = std::max(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 diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.cpp new file mode 100644 index 0000000..1b27307 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.cpp @@ -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; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.h new file mode 100644 index 0000000..a070d9c --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/ActiveSocket.h @@ -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__ */ + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/CMakeLists.txt new file mode 100644 index 0000000..a664baa --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/CMakeLists.txt @@ -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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.cpp new file mode 100644 index 0000000..4c3fb27 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.cpp @@ -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; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.h new file mode 100644 index 0000000..f5e6577 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/PassiveSocket.h @@ -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 + ///
\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__ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.cpp new file mode 100644 index 0000000..608392d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.cpp @@ -0,0 +1,1514 @@ +/*---------------------------------------------------------------------------*/ +/* */ +/* CSimpleSocket.cpp - CSimpleSocket 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 "SimpleSocket.h" +using namespace ydlidar; +using namespace ydlidar::core; +using namespace ydlidar::core::network; + + +static volatile bool m_WSAStartup = false; + +CSimpleSocket::CSimpleSocket(CSocketType nType) : + m_socket(INVALID_SOCKET), + m_socketErrno(CSimpleSocket::SocketInvalidSocket), + m_pBuffer(NULL), m_nBufferSize(0), m_nSocketDomain(AF_INET), + m_nSocketType(SocketTypeInvalid), m_nBytesReceived(-1), + m_nBytesSent(-1), m_nFlags(0), + m_bIsBlocking(true), m_open(false) { + SetConnectTimeout(DEFAULT_CONNECTION_TIMEOUT_SEC, + DEFAULT_CONNECTION_TIMEOUT_USEC); + memset(&m_stRecvTimeout, 0, sizeof(struct timeval)); + memset(&m_stSendTimeout, 0, sizeof(struct timeval)); + memset(&m_stLinger, 0, sizeof(struct linger)); + + switch (nType) { + //---------------------------------------------------------------------- + // Declare socket type stream - TCP + //---------------------------------------------------------------------- + case CSimpleSocket::SocketTypeTcp: { + m_nSocketDomain = AF_INET; + m_nSocketType = CSimpleSocket::SocketTypeTcp; + break; + } + + case CSimpleSocket::SocketTypeTcp6: { + m_nSocketDomain = AF_INET6; + m_nSocketType = CSimpleSocket::SocketTypeTcp6; + break; + } + + //---------------------------------------------------------------------- + // Declare socket type datagram - UDP + //---------------------------------------------------------------------- + case CSimpleSocket::SocketTypeUdp: { + m_nSocketDomain = AF_INET; + m_nSocketType = CSimpleSocket::SocketTypeUdp; + break; + } + + case CSimpleSocket::SocketTypeUdp6: { + m_nSocketDomain = AF_INET6; + m_nSocketType = CSimpleSocket::SocketTypeUdp6; + break; + } + + //---------------------------------------------------------------------- + // Declare socket type raw Ethernet - Ethernet + //---------------------------------------------------------------------- + case CSimpleSocket::SocketTypeRaw: { +#if defined(__linux__) && !defined(_DARWIN) + m_nSocketDomain = AF_PACKET; + m_nSocketType = CSimpleSocket::SocketTypeRaw; +#endif +#if defined(_WIN32) + m_nSocketType = CSimpleSocket::SocketTypeInvalid; +#endif + break; + } + + default: + m_nSocketType = CSimpleSocket::SocketTypeInvalid; + break; + } +} + +CSimpleSocket::CSimpleSocket(CSimpleSocket &socket) { + m_pBuffer = new uint8_t[socket.m_nBufferSize]; + m_nBufferSize = socket.m_nBufferSize; + memcpy(m_pBuffer, socket.m_pBuffer, socket.m_nBufferSize); +} + +CSimpleSocket *CSimpleSocket::operator=(CSimpleSocket &socket) { + if (m_nBufferSize != socket.m_nBufferSize) { + delete m_pBuffer; + m_pBuffer = new uint8_t[socket.m_nBufferSize]; + m_nBufferSize = socket.m_nBufferSize; + memcpy(m_pBuffer, socket.m_pBuffer, socket.m_nBufferSize); + } + + return this; +} + + +void CSimpleSocket::WSACleanUp() { +#if defined(_WIN32) + + if (m_WSAStartup) { + WSACleanup(); + m_WSAStartup = false; + } + +#endif +} + +bool CSimpleSocket::bindport(const char *addr, uint32_t port) { + m_addr = addr; + m_port = port; + bool was_open = this->isOpen(); + + if (was_open) { + closePort(); + } + + DisableNagleAlgoritm(); + SetConnectTimeout(DEFAULT_CONNECTION_TIMEOUT_SEC, + DEFAULT_CONNECTION_TIMEOUT_USEC); + SetReceiveTimeout(DEFAULT_REV_TIMEOUT_SEC, DEFAULT_REV_TIMEOUT_USEC); + SetSendTimeout(DEFAULT_REV_TIMEOUT_SEC, DEFAULT_REV_TIMEOUT_USEC); + return true; +} + +bool CSimpleSocket::open() { + if (!IsSocketValid()) { + if (!Initialize()) { + Close(); + m_open = false; + return false; + } + } + + SetNonblocking(); + m_open = Open(m_addr.c_str(), m_port); + + if (m_open) { + SetReceiveTimeout(DEFAULT_REV_TIMEOUT_SEC, DEFAULT_REV_TIMEOUT_USEC); + SetSendTimeout(DEFAULT_REV_TIMEOUT_SEC, DEFAULT_REV_TIMEOUT_USEC); + SetBlocking(); + } + + if (!m_open) { + Close(); + } + + return m_open; +} + +bool CSimpleSocket::isOpen() { + return (m_open || IsSocketValid()); +} + +void CSimpleSocket::closePort() { + Close(); + m_open = false; + +} + +void CSimpleSocket::flush() { + if (isOpen()) { + size_t size = 0; + int ret = waitfordata(1024, 2000, &size); + + if (size > 0) { + uint8_t *buf = static_cast(alloca(size * sizeof(uint8_t))); + readData(buf, size); + } + + } +} + +size_t CSimpleSocket::available() { + size_t returned_size = 0; + int max_fd; + FD_ZERO(&m_readFds); + FD_SET(m_socket, &m_readFds); + max_fd = static_cast(m_socket + 1); + + if (IsSocketValid()) { + if (IOCTLSOCKET(m_socket, FIONREAD, &returned_size) == -1) { + return 0; + } + } + + return returned_size; +} + +std::string CSimpleSocket::readSize(size_t size) { + std::string buffer; + uint8_t *buffer_ = static_cast(alloca(size * sizeof(uint8_t))); + size_t bytes_read = this->readData(buffer_, size); + buffer.append(reinterpret_cast(buffer_), bytes_read); + return buffer; +} + +int CSimpleSocket::waitfordata(size_t data_count, uint32_t timeout, + size_t *returned_size) { + return WaitForData(data_count, timeout, returned_size); +} + +size_t CSimpleSocket::writeData(const uint8_t *data, size_t size) { + int32_t sz = Send(data, size); + + if (sz < 0) { + return 0; + } + + return sz; +} + +size_t CSimpleSocket::readData(uint8_t *data, size_t size) { + int32_t rz = Receive(size, data); + + if (rz < 0) { + return 0; + } + + return rz; +} + + + + +//------------------------------------------------------------------------------ +// +// Initialize() - Initialize socket class +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::Initialize() { + errno = CSimpleSocket::SocketSuccess; + +#if defined(_WIN32) + + //------------------------------------------------------------------------- + // Data structure containing general Windows Sockets Info + //------------------------------------------------------------------------- + if (!m_WSAStartup) { + memset(&m_hWSAData, 0, sizeof(m_hWSAData)); + + if (WSAStartup(MAKEWORD(2, 2), &m_hWSAData) != 0) { + TranslateSocketError(); + return (IsSocketValid()); + } + + m_WSAStartup = true; + } + +#endif + + //------------------------------------------------------------------------- + // Create the basic Socket Handle + //------------------------------------------------------------------------- + m_timer.Initialize(); + m_timer.SetStartTime(); + m_socket = socket(m_nSocketDomain, m_nSocketType, 0); + m_timer.SetEndTime(); + + TranslateSocketError(); + + return (IsSocketValid()); +} + + +//------------------------------------------------------------------------------ +// +// BindInterface() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::BindInterface(const char *pInterface) { + bool bRetVal = false; + struct in_addr stInterfaceAddr; + + if (GetMulticast() == true) { + if (pInterface) { + stInterfaceAddr.s_addr = inet_addr(pInterface); + + if (SETSOCKOPT(m_socket, IPPROTO_IP, IP_MULTICAST_IF, &stInterfaceAddr, + sizeof(stInterfaceAddr)) == SocketSuccess) { + bRetVal = true; + } + } + } else { + SetSocketError(CSimpleSocket::SocketProtocolError); + } + + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// SetMulticast() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetMulticast(bool bEnable, uint8_t multicastTTL) { + bool bRetVal = false; + + if (GetSocketType() == CSimpleSocket::SocketTypeUdp) { + m_bIsMulticast = bEnable; + + if (SETSOCKOPT(m_socket, IPPROTO_IP, IP_MULTICAST_TTL, (void *)&multicastTTL, + sizeof(multicastTTL)) == SocketError) { + TranslateSocketError(); + bRetVal = false; + } else { + bRetVal = true; + } + } else { + m_socketErrno = CSimpleSocket::SocketProtocolError; + } + + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// SetSocketDscp() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetSocketDscp(int32_t nDscp) { + bool bRetVal = true; + int32_t nTempVal = nDscp; + nTempVal <<= 4; + nTempVal /= 4; + + if (IsSocketValid()) { + if (SETSOCKOPT(m_socket, IPPROTO_IP, IP_TOS, &nTempVal, + sizeof(nTempVal)) == SocketError) { + TranslateSocketError(); + bRetVal = false; + } + } + + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// GetSocketDscp() +// +//------------------------------------------------------------------------------ +int32_t CSimpleSocket::GetSocketDscp(void) { + int32_t nTempVal = 0; + socklen_t nLen = 0; + + if (IsSocketValid()) { + if (GETSOCKOPT(m_socket, IPPROTO_IP, IP_TOS, &nTempVal, &nLen) == SocketError) { + TranslateSocketError(); + } + + nTempVal *= 4; + nTempVal >>= 4; + } + + return nTempVal; +} + + +//------------------------------------------------------------------------------ +// +// GetWindowSize() +// +//------------------------------------------------------------------------------ +uint32_t CSimpleSocket::GetWindowSize(uint32_t nOptionName) { + uint32_t nTcpWinSize = 0; + + //------------------------------------------------------------------------- + // no socket given, return system default allocate our own new socket + //------------------------------------------------------------------------- + if (m_socket != CSimpleSocket::SocketError) { + socklen_t nLen = sizeof(nTcpWinSize); + //--------------------------------------------------------------------- + // query for buffer size + //--------------------------------------------------------------------- + GETSOCKOPT(m_socket, SOL_SOCKET, nOptionName, &nTcpWinSize, &nLen); + TranslateSocketError(); + } else { + SetSocketError(CSimpleSocket::SocketInvalidSocket); + } + + return nTcpWinSize; +} + + +//------------------------------------------------------------------------------ +// +// SetWindowSize() +// +//------------------------------------------------------------------------------ +uint32_t CSimpleSocket::SetWindowSize(uint32_t nOptionName, + uint32_t nWindowSize) { + uint32_t nRetVal = 0; + + //------------------------------------------------------------------------- + // no socket given, return system default allocate our own new socket + //------------------------------------------------------------------------- + if (m_socket != CSimpleSocket::SocketError) { + nRetVal = SETSOCKOPT(m_socket, SOL_SOCKET, nOptionName, &nWindowSize, + sizeof(nWindowSize)); + TranslateSocketError(); + } else { + SetSocketError(CSimpleSocket::SocketInvalidSocket); + } + + return nWindowSize; +} + + +//------------------------------------------------------------------------------ +// +// DisableNagleAlgorithm() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::DisableNagleAlgoritm() { + bool bRetVal = false; + int32_t nTcpNoDelay = 1; + + //---------------------------------------------------------------------- + // Set TCP NoDelay flag to true + //---------------------------------------------------------------------- + if (SETSOCKOPT(m_socket, IPPROTO_TCP, TCP_NODELAY, &nTcpNoDelay, + sizeof(int32_t)) == 0) { + bRetVal = true; + } + + TranslateSocketError(); + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// EnableNagleAlgorithm() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::EnableNagleAlgoritm() { + bool bRetVal = false; + int32_t nTcpNoDelay = 0; + + //---------------------------------------------------------------------- + // Set TCP NoDelay flag to false + //---------------------------------------------------------------------- + if (SETSOCKOPT(m_socket, IPPROTO_TCP, TCP_NODELAY, &nTcpNoDelay, + sizeof(int32_t)) == 0) { + bRetVal = true; + } + + TranslateSocketError(); + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// Send() - Send data on a valid socket +// +//------------------------------------------------------------------------------ +int32_t CSimpleSocket::Send(const uint8_t *pBuf, size_t bytesToSend) { + SetSocketError(SocketSuccess); + m_nBytesSent = 0; + + switch (m_nSocketType) { + case CSimpleSocket::SocketTypeTcp: { + if (IsSocketValid()) { + if ((bytesToSend > 0) && (pBuf != NULL)) { + m_timer.Initialize(); + m_timer.SetStartTime(); + + //--------------------------------------------------------- + // Check error condition and attempt to resend if call + // was interrupted by a signal. + //--------------------------------------------------------- + do { + m_timer.SetEndTime(); + + if (m_timer.GetMilliSeconds() > DEFAULT_REV_TIMEOUT_SEC * 1000) { + SetSocketError(CSimpleSocket::SocketTimedout); + break; + } + +#if defined(_WIN32) + m_nBytesSent = SEND(m_socket, pBuf, bytesToSend, 0); +#else + m_nBytesSent = SEND(m_socket, pBuf, bytesToSend, MSG_NOSIGNAL); +#endif + TranslateSocketError(); + + if (GetSocketError() == CSimpleSocket::SocketTimedout) { + m_timer.SetEndTime(); + + if (m_timer.GetMilliSeconds() > DEFAULT_REV_TIMEOUT_SEC * 1000) { + SetSocketError(CSimpleSocket::SocketTimedout); + break; + } + } + + if (GetSocketError() == CSimpleSocket::SocketEwouldblock) { + break; + } + } while (GetSocketError() == CSimpleSocket::SocketInterrupted); + + m_timer.SetEndTime(); + } + } + + break; + } + + case CSimpleSocket::SocketTypeUdp: { + if (IsSocketValid()) { + if ((bytesToSend > 0) && (pBuf != NULL)) { + m_timer.Initialize(); + m_timer.SetStartTime(); + + //--------------------------------------------------------- + // Check error condition and attempt to resend if call + // was interrupted by a signal. + //--------------------------------------------------------- + // if (GetMulticast()) + // { + // do + // { + // m_nBytesSent = SENDTO(m_socket, pBuf, bytesToSend, 0, (const sockaddr *)&m_stMulticastGroup, + // sizeof(m_stMulticastGroup)); + // TranslateSocketError(); + // } while (GetSocketError() == CSimpleSocket::SocketInterrupted); + // } + // else + { + do { + m_nBytesSent = SENDTO(m_socket, pBuf, bytesToSend, 0, + (const sockaddr *)&m_stServerSockaddr, + sizeof(m_stServerSockaddr)); + TranslateSocketError(); + } while (GetSocketError() == CSimpleSocket::SocketInterrupted); + } + + m_timer.SetEndTime(); + } + } + + break; + } + + default: + break; + } + + return m_nBytesSent; +} + + +//------------------------------------------------------------------------------ +// +// Close() - Close socket and free up any memory allocated for the socket +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::Close(void) { + bool bRetVal = false; + + //-------------------------------------------------------------------------- + // delete internal buffer + //-------------------------------------------------------------------------- + if (m_pBuffer != NULL) { + delete [] m_pBuffer; + m_pBuffer = NULL; + } + + //-------------------------------------------------------------------------- + // if socket handle is currently valid, close and then invalidate + //-------------------------------------------------------------------------- + if (IsSocketValid()) { + if (CLOSE(m_socket) != CSimpleSocket::SocketError) { + m_socket = INVALID_SOCKET; + bRetVal = true; + } + } + + TranslateSocketError(); + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// Shtudown() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::Shutdown(CShutdownMode nShutdown) { + CSocketError nRetVal = SocketEunknown; + nRetVal = (CSocketError)shutdown(m_socket, CSimpleSocket::Sends); + TranslateSocketError(); + return (nRetVal == CSimpleSocket::SocketSuccess) ? true : false; +} + + +//------------------------------------------------------------------------------ +// +// Flush() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::Flush() { + int32_t nTcpNoDelay = 1; + int32_t nCurFlags = 0; + uint8_t tmpbuf = 0; + bool bRetVal = false; + + if (!IsSocketValid()) { + return false; + } + + //-------------------------------------------------------------------------- + // Get the current setting of the TCP_NODELAY flag. + //-------------------------------------------------------------------------- + if (GETSOCKOPT(m_socket, IPPROTO_TCP, TCP_NODELAY, &nCurFlags, + sizeof(int32_t)) == 0) { + //---------------------------------------------------------------------- + // Set TCP NoDelay flag + //---------------------------------------------------------------------- + if (SETSOCKOPT(m_socket, IPPROTO_TCP, TCP_NODELAY, &nTcpNoDelay, + sizeof(int32_t)) == 0) { + //------------------------------------------------------------------ + // Send empty byte stream to flush the TCP send buffer + //------------------------------------------------------------------ + if (Send(&tmpbuf, 0) != CSimpleSocket::SocketError) { + bRetVal = true; + } + + TranslateSocketError(); + } + + //---------------------------------------------------------------------- + // Reset the TCP_NODELAY flag to original state. + //---------------------------------------------------------------------- + SETSOCKOPT(m_socket, IPPROTO_TCP, TCP_NODELAY, &nCurFlags, sizeof(int32_t)); + } + + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// Writev - +// +//------------------------------------------------------------------------------ +int32_t CSimpleSocket::Writev(const struct iovec *pVector, size_t nCount) { + int32_t nBytes = 0; + int32_t nBytesSent = 0; + int32_t i = 0; + + //-------------------------------------------------------------------------- + // Send each buffer as a separate send, windows does not support this + // function call. + //-------------------------------------------------------------------------- + for (i = 0; i < (int32_t)nCount; i++) { + if ((nBytes = Send((uint8_t *)pVector[i].iov_base, + pVector[i].iov_len)) == CSimpleSocket::SocketError) { + break; + } + + nBytesSent += nBytes; + } + + if (i > 0) { + Flush(); + } + + return nBytesSent; +} + + +//------------------------------------------------------------------------------ +// +// Send() - Send data on a valid socket via a vector of buffers. +// +//------------------------------------------------------------------------------ +int32_t CSimpleSocket::Send(const struct iovec *sendVector, int32_t nNumItems) { + SetSocketError(SocketSuccess); + m_nBytesSent = 0; + + if ((m_nBytesSent = WRITEV(m_socket, sendVector, + nNumItems)) == CSimpleSocket::SocketError) { + TranslateSocketError(); + } + + return m_nBytesSent; +} + + +//------------------------------------------------------------------------------ +// +// SetReceiveTimeout() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetReceiveTimeout(int32_t nRecvTimeoutSec, + int32_t nRecvTimeoutUsec) { + bool bRetVal = true; + memset(&m_stRecvTimeout, 0, sizeof(struct timeval)); + m_stRecvTimeout.tv_sec = nRecvTimeoutSec; + m_stRecvTimeout.tv_usec = nRecvTimeoutUsec; + + //-------------------------------------------------------------------------- + // Sanity check to make sure the options are supported! + //-------------------------------------------------------------------------- +#if defined(_WIN32) + int timeout = nRecvTimeoutSec * 1000 + nRecvTimeoutUsec / 1000; + + if (SETSOCKOPT(m_socket, SOL_SOCKET, SO_RCVTIMEO, &timeout, + sizeof(timeout)) == CSimpleSocket::SocketError) { + bRetVal = false; + TranslateSocketError(); + } + +#else + + if (SETSOCKOPT(m_socket, SOL_SOCKET, SO_RCVTIMEO, &m_stRecvTimeout, + sizeof(struct timeval)) == CSimpleSocket::SocketError) { + bRetVal = false; + TranslateSocketError(); + } + +#endif + + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// SetSendTimeout() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetSendTimeout(int32_t nSendTimeoutSec, + int32_t nSendTimeoutUsec) { + bool bRetVal = true; + memset(&m_stSendTimeout, 0, sizeof(struct timeval)); + m_stSendTimeout.tv_sec = nSendTimeoutSec; + m_stSendTimeout.tv_usec = nSendTimeoutUsec; + + //-------------------------------------------------------------------------- + // Sanity check to make sure the options are supported! + //-------------------------------------------------------------------------- +#if defined(_WIN32) + int timeout = nSendTimeoutSec * 1000 + nSendTimeoutUsec / 1000; + + if (SETSOCKOPT(m_socket, SOL_SOCKET, SO_SNDTIMEO, &timeout, + sizeof(timeout)) == CSimpleSocket::SocketError) { + bRetVal = false; + TranslateSocketError(); + } + +#else + + if (SETSOCKOPT(m_socket, SOL_SOCKET, SO_SNDTIMEO, &m_stSendTimeout, + sizeof(struct timeval)) == CSimpleSocket::SocketError) { + bRetVal = false; + TranslateSocketError(); + } + +#endif + + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// SetOptionReuseAddr() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetOptionReuseAddr() { + bool bRetVal = false; + int32_t nReuse = IPTOS_LOWDELAY; + + if (SETSOCKOPT(m_socket, SOL_SOCKET, SO_REUSEADDR, (char *)&nReuse, + sizeof(int32_t)) == 0) { + bRetVal = true; + } + + TranslateSocketError(); + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// SetOptionLinger() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetOptionLinger(bool bEnable, uint16_t nTime) { + bool bRetVal = false; + m_stLinger.l_onoff = (bEnable == true) ? 1 : 0; + m_stLinger.l_linger = nTime; + + if (SETSOCKOPT(m_socket, SOL_SOCKET, SO_LINGER, &m_stLinger, + sizeof(m_stLinger)) == 0) { + bRetVal = true; + } + + TranslateSocketError(); + return bRetVal; +} + + +//------------------------------------------------------------------------------ +// +// Receive() - Attempts to receive a block of data on an established +// connection. Data is received in an internal buffer managed +// by the class. This buffer is only valid until the next call +// to Receive(), a call to Close(), or until the object goes out +// of scope. +// +//------------------------------------------------------------------------------ +int32_t CSimpleSocket::Receive(int32_t nMaxBytes, uint8_t *pBuffer) { + m_nBytesReceived = 0; + + //-------------------------------------------------------------------------- + // If the socket is invalid then return false. + //-------------------------------------------------------------------------- + if (IsSocketValid() == false) { + return m_nBytesReceived; + } + + uint8_t *pWorkBuffer = pBuffer; + + if (pBuffer == NULL) { + //-------------------------------------------------------------------------- + // Free existing buffer and allocate a new buffer the size of + // nMaxBytes. + //-------------------------------------------------------------------------- + if ((m_pBuffer != NULL) && (nMaxBytes != m_nBufferSize)) { + delete [] m_pBuffer; + m_pBuffer = NULL; + } + + //-------------------------------------------------------------------------- + // Allocate a new internal buffer to receive data. + //-------------------------------------------------------------------------- + if (m_pBuffer == NULL) { + m_nBufferSize = nMaxBytes; + m_pBuffer = new uint8_t[nMaxBytes]; + } + + pWorkBuffer = m_pBuffer; + } + + SetSocketError(SocketSuccess); + + m_timer.Initialize(); + m_timer.SetStartTime(); + + switch (m_nSocketType) { + //---------------------------------------------------------------------- + // If zero bytes are received, then return. If SocketERROR is + // received, free buffer and return CSocket::SocketError (-1) to caller. + //---------------------------------------------------------------------- + case CSimpleSocket::SocketTypeTcp: { + do { + m_timer.SetEndTime(); + + if (m_timer.GetMilliSeconds() > DEFAULT_REV_TIMEOUT_SEC * 1000) { + SetSocketError(CSimpleSocket::SocketTimedout); + break; + } + + m_nBytesReceived = RECV(m_socket, (pWorkBuffer + m_nBytesReceived), + nMaxBytes, m_nFlags); + TranslateSocketError(); + + if (m_nBytesReceived >= nMaxBytes) { + break; + } + + if (m_nBytesReceived <= 0) { + if (GetSocketError() == CSimpleSocket::SocketTimedout) { + m_timer.SetEndTime(); + + if (m_timer.GetMilliSeconds() > DEFAULT_REV_TIMEOUT_SEC * 1000) { + SetSocketError(CSimpleSocket::SocketTimedout); + break; + } + } + } + + if (GetSocketError() == CSimpleSocket::SocketEwouldblock) { + m_nBytesReceived = -2; + break; + } + } while ((GetSocketError() == CSimpleSocket::SocketInterrupted)); + + break; + } + + case CSimpleSocket::SocketTypeUdp: { + uint32_t srcSize; + srcSize = sizeof(struct sockaddr_in); + + if (GetMulticast() == true) { + do { + m_timer.SetEndTime(); + + if (m_timer.GetMilliSeconds() > DEFAULT_REV_TIMEOUT_SEC * 1000) { + SetSocketError(CSimpleSocket::SocketTimedout); + break; + } + + m_nBytesReceived = RECVFROM(m_socket, pWorkBuffer, nMaxBytes, 0, + &m_stMulticastGroup, &srcSize); + TranslateSocketError(); + + if (m_nBytesReceived >= nMaxBytes) { + break; + } + } while (GetSocketError() == CSimpleSocket::SocketInterrupted); + } else { + do { + m_timer.SetEndTime(); + + if (m_timer.GetMilliSeconds() > DEFAULT_REV_TIMEOUT_SEC * 1000) { + SetSocketError(CSimpleSocket::SocketTimedout); + break; + } + + m_nBytesReceived = RECVFROM(m_socket, pWorkBuffer, nMaxBytes, 0, + &m_stClientSockaddr, &srcSize); + TranslateSocketError(); + + if (m_nBytesReceived >= nMaxBytes) { + break; + } + + if (GetSocketError() != CSimpleSocket::SocketSuccess) { + break; + } + } while (GetSocketError() == CSimpleSocket::SocketInterrupted); + } + + break; + } + + default: + break; + } + + m_timer.SetEndTime(); + TranslateSocketError(); + + //-------------------------------------------------------------------------- + // If we encounter an error translate the error code and return. One + // possible error code could be EAGAIN (EWOULDBLOCK) if the socket is + // non-blocking. This does not mean there is an error, but no data is + // yet available on the socket. + //-------------------------------------------------------------------------- + if (m_nBytesReceived == CSimpleSocket::SocketError) { + if (m_pBuffer != NULL) { + delete [] m_pBuffer; + m_pBuffer = NULL; + } + } + + return m_nBytesReceived; +} + + +//------------------------------------------------------------------------------ +// +// SetNonblocking() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetNonblocking(void) { + int32_t nCurFlags; +#if defined(_WIN32) + nCurFlags = 1; + + if (ioctlsocket(m_socket, FIONBIO, (ULONG *)&nCurFlags) != 0) { + TranslateSocketError(); + return false; + } + +#else + + if ((nCurFlags = fcntl(m_socket, F_GETFL)) < 0) { + TranslateSocketError(); + return false; + } + + nCurFlags |= O_NONBLOCK; + + if (fcntl(m_socket, F_SETFL, nCurFlags) != 0) { + TranslateSocketError(); + return false; + } + +#endif + m_bIsBlocking = false; + return true; +} + + +//------------------------------------------------------------------------------ +// +// SetBlocking() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::SetBlocking(void) { + int32_t nCurFlags; +#if defined(_WIN32) + nCurFlags = 0; + + if (ioctlsocket(m_socket, FIONBIO, (ULONG *)&nCurFlags) != 0) { + return false; + } + +#else + + if ((nCurFlags = fcntl(m_socket, F_GETFL)) < 0) { + TranslateSocketError(); + return false; + } + + nCurFlags &= (~O_NONBLOCK); + + if (fcntl(m_socket, F_SETFL, nCurFlags) != 0) { + TranslateSocketError(); + return false; + } + +#endif + m_bIsBlocking = true; + return true; +} + + +//------------------------------------------------------------------------------ +// +// SendFile() - stands-in for system provided sendfile +// +//------------------------------------------------------------------------------ +int32_t CSimpleSocket::SendFile(int32_t nOutFd, int32_t nInFd, off_t *pOffset, + int32_t nCount) { + int32_t nOutCount = CSimpleSocket::SocketError; + static char szData[SOCKET_SENDFILE_BLOCKSIZE]; + int32_t nInCount = 0; + + if (lseek(nInFd, *pOffset, SEEK_SET) == -1) { + return -1; + } + + while (nOutCount < nCount) { + nInCount = (nCount - nOutCount) < SOCKET_SENDFILE_BLOCKSIZE ? + (nCount - nOutCount) : + SOCKET_SENDFILE_BLOCKSIZE; + + if ((read(nInFd, szData, nInCount)) != (int32_t)nInCount) { + return -1; + } + + if ((SEND(nOutFd, szData, nInCount, 0)) != (int32_t)nInCount) { + return -1; + } + + nOutCount += nInCount; + } + + *pOffset += nOutCount; + TranslateSocketError(); + return nOutCount; +} + + +//------------------------------------------------------------------------------ +// +// TranslateSocketError() - +// +//------------------------------------------------------------------------------ +void CSimpleSocket::TranslateSocketError(void) { +#if defined(__linux__) || defined(_DARWIN) + + switch (errno) { + case EXIT_SUCCESS: + SetSocketError(CSimpleSocket::SocketSuccess); + break; + + case ENOTCONN: + SetSocketError(CSimpleSocket::SocketNotconnected); + break; + + case ENOTSOCK: + case EBADF: + case EACCES: + case EAFNOSUPPORT: + case EMFILE: + case ENFILE: + case ENOBUFS: + case ENOMEM: + case EPROTONOSUPPORT: + case EPIPE: + SetSocketError(CSimpleSocket::SocketInvalidSocket); + break; + + case ECONNREFUSED : + SetSocketError(CSimpleSocket::SocketConnectionRefused); + break; + + case ETIMEDOUT: + SetSocketError(CSimpleSocket::SocketTimedout); + break; + + case EINPROGRESS: + SetSocketError(CSimpleSocket::SocketEinprogress); + break; + + case EWOULDBLOCK: + // case EAGAIN: + SetSocketError(CSimpleSocket::SocketEwouldblock); + break; + + case EINTR: + SetSocketError(CSimpleSocket::SocketInterrupted); + break; + + case ECONNABORTED: + SetSocketError(CSimpleSocket::SocketConnectionAborted); + break; + + case EINVAL: + case EPROTO: + SetSocketError(CSimpleSocket::SocketProtocolError); + break; + + case EPERM: + SetSocketError(CSimpleSocket::SocketFirewallError); + break; + + case EFAULT: + SetSocketError(CSimpleSocket::SocketInvalidSocketBuffer); + break; + + case ECONNRESET: + case ENOPROTOOPT: + SetSocketError(CSimpleSocket::SocketConnectionReset); + break; + + default: + SetSocketError(CSimpleSocket::SocketEunknown); + break; + } + +#endif +#if defined(_WIN32) + int32_t nError = WSAGetLastError(); + + switch (nError) { + case EXIT_SUCCESS: + SetSocketError(CSimpleSocket::SocketSuccess); + break; + + case WSAEBADF: + case WSAENOTCONN: + SetSocketError(CSimpleSocket::SocketNotconnected); + break; + + case WSAEINTR: + SetSocketError(CSimpleSocket::SocketInterrupted); + break; + + case WSAEACCES: + case WSAEAFNOSUPPORT: + case WSAEINVAL: + case WSAEMFILE: + case WSAENOBUFS: + case WSAEPROTONOSUPPORT: + SetSocketError(CSimpleSocket::SocketInvalidSocket); + break; + + case WSAECONNREFUSED : + SetSocketError(CSimpleSocket::SocketConnectionRefused); + break; + + case WSAETIMEDOUT: + SetSocketError(CSimpleSocket::SocketTimedout); + break; + + case WSAEINPROGRESS: + SetSocketError(CSimpleSocket::SocketEinprogress); + break; + + case WSAECONNABORTED: + SetSocketError(CSimpleSocket::SocketConnectionAborted); + break; + + case WSAEWOULDBLOCK: + SetSocketError(CSimpleSocket::SocketEwouldblock); + break; + + case WSAENOTSOCK: + SetSocketError(CSimpleSocket::SocketInvalidSocket); + break; + + case WSAECONNRESET: + SetSocketError(CSimpleSocket::SocketConnectionReset); + break; + + case WSANO_DATA: + SetSocketError(CSimpleSocket::SocketInvalidAddress); + break; + + case WSAEADDRINUSE: + SetSocketError(CSimpleSocket::SocketAddressInUse); + break; + + case WSAEFAULT: + SetSocketError(CSimpleSocket::SocketInvalidPointer); + break; + + default: + SetSocketError(CSimpleSocket::SocketEunknown); + break; + } + +#endif +} + +//------------------------------------------------------------------------------ +// +// DescribeError() +// +//------------------------------------------------------------------------------ + +const char *CSimpleSocket::DescribeError(CSocketError err) { + switch (err) { + case CSimpleSocket::SocketError: + return "Generic socket error translates to error below."; + + case CSimpleSocket::SocketSuccess: + return "No socket error."; + + case CSimpleSocket::SocketInvalidSocket: + return "Invalid socket handle."; + + case CSimpleSocket::SocketInvalidAddress: + return "Invalid destination address specified."; + + case CSimpleSocket::SocketInvalidPort: + return "Invalid destination port specified."; + + case CSimpleSocket::SocketConnectionRefused: + return "No server is listening at remote address."; + + case CSimpleSocket::SocketTimedout: + return "Timed out while attempting operation."; + + case CSimpleSocket::SocketEwouldblock: + return "Operation would block if socket were blocking."; + + case CSimpleSocket::SocketNotconnected: + return "Currently not connected."; + + case CSimpleSocket::SocketEinprogress: + return "Socket is non-blocking and the connection cannot be completed immediately"; + + case CSimpleSocket::SocketInterrupted: + return "Call was interrupted by a signal that was caught before a valid connection arrived."; + + case CSimpleSocket::SocketConnectionAborted: + return "The connection has been aborted."; + + case CSimpleSocket::SocketProtocolError: + return "Invalid protocol for operation."; + + case CSimpleSocket::SocketFirewallError: + return "Firewall rules forbid connection."; + + case CSimpleSocket::SocketInvalidSocketBuffer: + return "The receive buffer point outside the process's address space."; + + case CSimpleSocket::SocketConnectionReset: + return "Connection was forcibly closed by the remote host."; + + case CSimpleSocket::SocketAddressInUse: + return "Address already in use."; + + case CSimpleSocket::SocketInvalidPointer: + return "Pointer type supplied as argument is invalid."; + + case CSimpleSocket::SocketEunknown: + return "Unknown error"; + + default: + return "No such CSimpleSocket error"; + } +} + +//------------------------------------------------------------------------------ +// +// Select() +// +//------------------------------------------------------------------------------ +bool CSimpleSocket::Select(int32_t nTimeoutSec, int32_t nTimeoutUSec) { + bool bRetVal = false; + struct timeval *pTimeout = NULL; + struct timeval timeout; + int32_t nNumDescriptors = -1; + int32_t nError = 0; + + FD_ZERO(&m_errorFds); + FD_ZERO(&m_readFds); + FD_ZERO(&m_writeFds); + FD_SET(m_socket, &m_errorFds); + FD_SET(m_socket, &m_readFds); + FD_SET(m_socket, &m_writeFds); + + //--------------------------------------------------------------------- + // If timeout has been specified then set value, otherwise set timeout + // to NULL which will block until a descriptor is ready for read/write + // or an error has occurred. + //--------------------------------------------------------------------- + if ((nTimeoutSec > 0) || (nTimeoutUSec > 0)) { + timeout.tv_sec = nTimeoutSec; + timeout.tv_usec = nTimeoutUSec; + pTimeout = &timeout; + } + + nNumDescriptors = SELECT(m_socket + 1, &m_readFds, &m_writeFds, &m_errorFds, + pTimeout); +// nNumDescriptors = SELECT(m_socket+1, &m_readFds, NULL, NULL, pTimeout); + + //---------------------------------------------------------------------- + // Handle timeout + //---------------------------------------------------------------------- + if (nNumDescriptors == 0) { + SetSocketError(CSimpleSocket::SocketTimedout); + } + //---------------------------------------------------------------------- + // If a file descriptor (read/write) is set then check the + // socket error (SO_ERROR) to see if there is a pending error. + //---------------------------------------------------------------------- + else if ((FD_ISSET(m_socket, &m_readFds)) || + (FD_ISSET(m_socket, &m_writeFds))) { + int32_t nLen = sizeof(nError); + + if (GETSOCKOPT(m_socket, SOL_SOCKET, SO_ERROR, &nError, &nLen) == 0) { + errno = nError; + + if (nError == 0) { + bRetVal = true; + } + } + + TranslateSocketError(); + } + + return bRetVal; +} + +//------------------------------------------------------------------------------ +// +// WaitForData() +// +//------------------------------------------------------------------------------ + +int CSimpleSocket::WaitForData(size_t data_count, uint32_t timeout, + size_t *returned_size) { + size_t length = 0; + + if (returned_size == NULL) { + returned_size = (size_t *)&length; + } + + *returned_size = 0; + int max_fd; + struct timeval timeout_val; + FD_ZERO(&m_readFds); + FD_SET(m_socket, &m_readFds); + max_fd = static_cast(m_socket + 1); + int32_t nNumDescriptors = -1; + int32_t nError = 0; + + /* Initialize the timeout structure */ + timeout_val.tv_sec = timeout / 1000; + timeout_val.tv_usec = (timeout % 1000) * 1000; + + if (IsSocketValid()) { + if (IOCTLSOCKET(m_socket, FIONREAD, returned_size) == -1) { + return -2; + } + + if (*returned_size >= data_count) { + return 0; + } + } + + m_timer.Initialize(); + m_timer.SetStartTime(); + + while (IsSocketValid()) { + m_timer.SetEndTime(); + + if (m_timer.GetMilliSeconds() > timeout) { + SetSocketError(CSimpleSocket::SocketTimedout); + return -1; + } + + nNumDescriptors = SELECT(max_fd, &m_readFds, NULL, NULL, &timeout_val); + + if (nNumDescriptors < 0) { + if (FD_ISSET(m_socket, &m_readFds)) { + int32_t nLen = sizeof(nError); + int bRetVal = -2; + + if (GETSOCKOPT(m_socket, SOL_SOCKET, SO_ERROR, &nError, &nLen) == 0) { + errno = nError; + + if (nError == 0) { + bRetVal = -1; + } + } + + TranslateSocketError(); + return bRetVal; + } + } else if (nNumDescriptors == 0) { + // time out + SetSocketError(CSimpleSocket::SocketTimedout); + return -1; + } else { + // data avaliable + assert(FD_ISSET(m_socket, &m_readFds)); +#ifdef _WIN32 + + if (m_nSocketType == CSimpleSocket::SocketTypeTcp || + m_nSocketType == CSimpleSocket::SocketTypeUdp) { + if (returned_size) { + *returned_size = data_count; + } + + return 0; + } + +#endif + + if (m_nSocketType == CSimpleSocket::SocketTypeUdp) { + if (returned_size) { + *returned_size = data_count; + } + + m_timer.SetEndTime(); + return 0; + } + + if (IOCTLSOCKET(m_socket, FIONREAD, returned_size) == -1) { + int32_t nLen = sizeof(nError); + + if (GETSOCKOPT(m_socket, SOL_SOCKET, SO_ERROR, &nError, &nLen) == 0) { + errno = nError; + } + + TranslateSocketError(); + m_timer.SetEndTime(); + return -2; + } + + if (*returned_size >= data_count) { + m_timer.SetEndTime(); + return 0; + } else { + std::this_thread::sleep_for(std::chrono::microseconds(5)); + } + } + } + + m_timer.SetEndTime(); + return -2; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.h new file mode 100644 index 0000000..fb79b6a --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/SimpleSocket.h @@ -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 +#include +#include +#include +#include +#include +#include +#include + + +#if defined(__linux__) || defined (_DARWIN) +#include +#include +#include +#include +#include +#include +#endif +#ifdef __linux__ +#include +#include +#include +#include +#endif +#ifdef _DARWIN +#include +#endif +#if defined(__linux__) || defined (_DARWIN) +#include +#include +#include +#include +#include +#endif + +#if defined(_WIN32) +#include +#include +#include + +#pragma warning(disable: 4786) +#pragma comment(lib, "ws2_32.lib") +#pragma comment(lib, "setupapi.lib") +#define IPTOS_LOWDELAY 0x10 + +#endif +#include "StatTimer.h" +#include + +//----------------------------------------------------------------------------- +// 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 + ///
\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. + ///
\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). + ///

+ /// @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. + ///

\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 + ///

\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. + ///

\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. + ///

\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. + ///

\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. + ///

\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 + ///
\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__ */ + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/StatTimer.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/StatTimer.h new file mode 100644 index 0000000..a4a5527 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/network/StatTimer.h @@ -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 + +#if defined(_WIN32) +#include +#include +#else +#include +#include +#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 + + +/// 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__ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/CMakeLists.txt new file mode 100644 index 0000000..023cc81 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/CMakeLists.txt @@ -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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/common.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/common.h new file mode 100644 index 0000000..9a4b1c4 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/common.h @@ -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 +#include +#include + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/CMakeLists.txt new file mode 100644 index 0000000..cae8ed3 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/CMakeLists.txt @@ -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() + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/CMakeLists.txt new file mode 100644 index 0000000..a224b85 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/CMakeLists.txt @@ -0,0 +1,4 @@ +aux_include_directory(. HDRS) +aux_src_directory(. SRCS) +add_to_ydlidar_headers(${HDRS}) +add_to_ydlidar_sources(${SRCS}) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/list_ports_linux.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/list_ports_linux.cpp new file mode 100644 index 0000000..d0877ed --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/list_ports_linux.cpp @@ -0,0 +1,331 @@ +#if defined(__linux__) + +/* + * Copyright (c) 2014 Craig Lilley + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +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 glob(const vector &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 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 +glob(const vector &patterns) { + vector 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::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 +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 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 +serial::list_ports() +{ + vector results; + + vector 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 devices_found = glob(search_globs); + + vector::iterator iter = devices_found.begin(); + + while (iter != devices_found.end()) { + string device = *iter++; + + vector 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__) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.c b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.c new file mode 100644 index 0000000..87f4891 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.c @@ -0,0 +1,796 @@ +/* + * lock.c + * + * Created on: 28 Mar 2018 + * Author: Tony + * E-Mail: Tony@gmail.com + */ +#include "lock.h" + +#ifndef WIN32 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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.. + 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 +----------------------------------------------------------*/ +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 */ + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.h new file mode 100644 index 0000000..e844149 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/lock.h @@ -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 +#endif /* HAVE_SYS_FILE_H */ +#ifdef LFS /* File Lock Server */ +# include +# include +# include +#endif /* FLS */ +#if defined(__linux__) +# include /* fix for linux-2.3.4? kernels */ +# include +# include +#endif /* __linux__ */ +#if defined(__sun__) +# include +# include +#endif /* __sun__ */ +#if defined(__hpux__) +# include +#endif /* __hpux__ */ +/* FIXME -- new file */ +#if defined(__APPLE__) +# include +# include +# include +# include +#endif /* __APPLE__ */ +#ifdef __unixware__ +# include +#endif /* __unixware__ */ +#ifdef HAVE_PWD_H +#include +#endif /* HAVE_PWD_H */ +#ifdef HAVE_GRP_H +#include +#endif /* HAVE_GRP_H */ +#include +#ifdef LIBLOCKDEV +#include +#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 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_ */ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix.h new file mode 100644 index 0000000..e78f84d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix.h @@ -0,0 +1,27 @@ +#pragma once +// libc dep +#include +#include +#include +#include +#include +#include +#include +#include + +// libc++ dep +#include +#include + +// linux specific +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.cpp new file mode 100644 index 0000000..2c7266f --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.cpp @@ -0,0 +1,1634 @@ +#if !defined(_WIN32) + +#include +#include +#include +#include +#include +#include +#if !defined(__ANDROID__) +#include +#include + +#endif + +#include +#include +#include +#include +#include +#include + +#include + +#if defined(__linux__) &&!defined(__ANDROID__) +# include +#endif + +#include +#include +#include +#include + +#ifdef __MACH__ +#include +#include +#include +#endif + +#include "unix_serial.h" +#ifdef USE_LOCK_FILE +#include +#endif + + +#ifndef TIOCINQ +#ifdef FIONREAD +#define TIOCINQ FIONREAD +#else +#define TIOCINQ 0x541B +#endif +#endif + +#if defined(MAC_OS_X_VERSION_10_3) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_3) +#include +#endif + +/** +* setup_port - Configure the port, eg. baud rate, data bits,etc. +* +* @param fd : The serial port +* @param speed : The baud rate +* @param data_bits : The data bits +* @param parity : The parity bits +* @param stop_bits : The stop bits +* +* @return Return 0 if everything is OK, otherwise -1 with some error msg. +* @note +Here are termios structure members: +\verbatim +Member Description +c_cflag Control options +c_lflag Line options +c_iflag Input options +c_oflag Output options +c_cc Control characters +c_ispeed Input baud (new interface) +c_ospeed Output baud (new interface) +\endverbatim +The c_cflag member controls the baud rate, number of data bits, parity, +stop bits, and hardware flow control. There are constants for all of the +supported configurations. +Constant Description +\verbatim +CBAUD Bit mask for baud rate +B0 0 baud (drop DTR) +B50 50 baud +B75 75 baud +B110 110 baud +B134 134.5 baud +B150 150 baud +B200 200 baud +B300 300 baud +B600 600 baud +B1200 1200 baud +B1800 1800 baud +B2400 2400 baud +B4800 4800 baud +B9600 9600 baud +B19200 19200 baud +B38400 38400 baud +B57600 57,600 baud +B76800 76,800 baud +B115200 115,200 baud +EXTA External rate clock +EXTB External rate clock +CSIZE Bit mask for data bits +CS5 5 data bits +CS6 6 data bits +CS7 7 data bits +CS8 8 data bits +CSTOPB 2 stop bits (1 otherwise) +CREAD Enable receiver +PARENB Enable parity bit +PARODD Use odd parity instead of even +HUPCL Hangup (drop DTR) on last close +CLOCAL Local line - do not change "owner" of port +LOBLK Block job control output +CNEW_RTSCTS CRTSCTS Enable hardware flow control (not supported on all +platforms) +\endverbatim +The input modes member c_iflag controls any input processing that is done to +characters received on the port. Like the c_cflag field, the final value +stored in c_iflag is the bitwise OR of the desired options. +\verbatim +Constant Description +INPCK Enable parity check +IGNPAR Ignore parity errors +PARMRK Mark parity errors +ISTRIP Strip parity bits +IXON Enable software flow control (outgoing) +IXOFF Enable software flow control (incoming) +IXANY Allow any character to start flow again +IGNBRK Ignore break condition +BRKINT Send a SIGINT when a break condition is detected +INLCR Map NL to CR +IGNCR Ignore CR +ICRNL Map CR to NL +IUCLC Map uppercase to lowercase +IMAXBEL Echo BEL on input line too long +\endverbatim +Here are some examples of setting parity checking: @n +No parity (8N1): +\verbatim +options.c_cflag &= ~PARENB +options.c_cflag &= ~CSTOPB +options.c_cflag &= ~CSIZE; +options.c_cflag |= CS8; +\endverbatim +Even parity (7E1): +\verbatim +options.c_cflag |= PARENB +options.c_cflag &= ~PARODD +options.c_cflag &= ~CSTOPB +options.c_cflag &= ~CSIZE; +options.c_cflag |= CS7; +\endverbatim +Odd parity (7O1): +\verbatim +options.c_cflag |= PARENB +options.c_cflag |= PARODD +options.c_cflag &= ~CSTOPB +options.c_cflag &= ~CSIZE; +options.c_cflag |= CS7; +\endverbatim +*/ + +namespace ydlidar { +namespace core { +namespace serial { + +using std::string; +using serial::MillisecondTimer; +using serial::Serial; +using namespace serial; + +#define SNCCS 19 + + +struct termios2 { + tcflag_t c_iflag; /* input mode flags */ + tcflag_t c_oflag; /* output mode flags */ + tcflag_t c_cflag; /* control mode flags */ + tcflag_t c_lflag; /* local mode flags */ + cc_t c_line; /* line discipline */ + cc_t c_cc[SNCCS]; /* control characters */ + speed_t c_ispeed; /* input speed */ + speed_t c_ospeed; /* output speed */ +}; + +#ifndef TCGETS2 +#define TCGETS2 _IOR('T', 0x2A, struct termios2) +#endif + +#ifndef TCSETS2 +#define TCSETS2 _IOW('T', 0x2B, struct termios2) +#endif + +#ifndef BOTHER +# define BOTHER 0010000 +#endif + + +#if defined(__ANDROID__) +struct serial_struct { + int type; + int line; + unsigned int port; + int irq; + int flags; + int xmit_fifo_size; + int custom_divisor; + int baud_base; + unsigned short close_delay; + char io_type; + char reserved_char[1]; + int hub6; + unsigned short closing_wait; + unsigned short closing_wait2; + unsigned char *iomem_base; + unsigned short iomem_reg_shift; + unsigned int port_high; + unsigned long iomap_base; +}; +# define ASYNC_SPD_CUST 0x0030 +# define ASYNC_SPD_MASK 0x1030 +# define PORT_UNKNOWN 0 +# define FNDELAY 0x800 +#endif + + +MillisecondTimer::MillisecondTimer(const uint32_t millis) : expiry( + timespec_now()) { + int64_t tv_nsec = expiry.tv_nsec + (millis * 1e6); + + if (tv_nsec >= 1e9) { + int64_t sec_diff = tv_nsec / static_cast(1e9); + expiry.tv_nsec = tv_nsec % static_cast(1e9); + expiry.tv_sec += sec_diff; + } else { + expiry.tv_nsec = tv_nsec; + } +} + +int64_t MillisecondTimer::remaining() { + timespec now(timespec_now()); + int64_t millis = (expiry.tv_sec - now.tv_sec) * 1e3; + millis += (expiry.tv_nsec - now.tv_nsec) / 1e6; + return millis; +} + +timespec MillisecondTimer::timespec_now() { + timespec time; +# ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + time.tv_sec = mts.tv_sec; + time.tv_nsec = mts.tv_nsec; +# else + clock_gettime(CLOCK_MONOTONIC, &time); +# endif + return time; +} + +timespec timespec_from_ms(const uint32_t millis) { + timespec time; + time.tv_sec = millis / 1e3; + time.tv_nsec = (millis - (time.tv_sec * 1e3)) * 1e6; + return time; +} + + +static inline void set_common_props(termios *tio) { +#ifdef OS_SOLARIS + tio->c_iflag &= ~(IMAXBEL | IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | + ICRNL | IXON); + tio->c_oflag &= ~OPOST; + tio->c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + tio->c_cflag &= ~(CSIZE | PARENB); + tio->c_cflag |= CS8; +#else + ::cfmakeraw(tio); +#endif + tio->c_cflag |= CLOCAL | CREAD; + tio->c_cc[VTIME] = 0; + tio->c_cc[VMIN] = 0; +} + + +static inline void set_databits(termios *tio, serial::bytesize_t databits) { + tio->c_cflag &= ~CSIZE; + + switch (databits) { + case serial::fivebits: + tio->c_cflag |= CS5; + break; + + case serial::sixbits: + tio->c_cflag |= CS6; + break; + + case serial::sevenbits: + tio->c_cflag |= CS7; + break; + + case serial::eightbits: + tio->c_cflag |= CS8; + break; + + default: + tio->c_cflag |= CS8; + break; + } +} + + +static inline void set_parity(termios *tio, serial::parity_t parity) { + tio->c_iflag &= ~(PARMRK | INPCK); + tio->c_iflag |= IGNPAR; + + switch (parity) { + +#ifdef CMSPAR + + // Here Installation parity only for GNU/Linux where the macro CMSPAR. + case serial::parity_space: + tio->c_cflag &= ~PARODD; + tio->c_cflag |= PARENB | CMSPAR; + break; + + case serial::parity_mark: + tio->c_cflag |= PARENB | CMSPAR | PARODD; + break; +#endif + + case serial::parity_none: + tio->c_cflag &= ~PARENB; + break; + + case serial::parity_even: + tio->c_cflag &= ~PARODD; + tio->c_cflag |= PARENB; + break; + + case serial::parity_odd: + tio->c_cflag |= PARENB | PARODD; + break; + + default: + tio->c_cflag |= PARENB; + tio->c_iflag |= PARMRK | INPCK; + tio->c_iflag &= ~IGNPAR; + break; + } +} + + +static inline void set_stopbits(termios *tio, serial::stopbits_t stopbits) { + switch (stopbits) { + case serial::stopbits_one: + tio->c_cflag &= ~CSTOPB; + break; + + case serial::stopbits_two: + tio->c_cflag |= CSTOPB; + break; + + default: + tio->c_cflag &= ~CSTOPB; + break; + } +} + +static inline void set_flowcontrol(termios *tio, + serial::flowcontrol_t flowcontrol) { + switch (flowcontrol) { + case serial::flowcontrol_none: + tio->c_cflag &= ~CRTSCTS; + tio->c_iflag &= ~(IXON | IXOFF | IXANY); + break; + + case serial::flowcontrol_hardware: + tio->c_cflag |= CRTSCTS; + tio->c_iflag &= ~(IXON | IXOFF | IXANY); + break; + + case serial::flowcontrol_software: + tio->c_cflag &= ~CRTSCTS; + tio->c_iflag |= IXON | IXOFF | IXANY; + break; + + default: + tio->c_cflag &= ~CRTSCTS; + tio->c_iflag &= ~(IXON | IXOFF | IXANY); + break; + } +} + + +static inline bool is_standardbaudrate(unsigned long baudrate, speed_t &baud) { + // setup baud rate + bool custom_baud = false; + + switch (baudrate) { +#ifdef B0 + + case 0: + baud = B0; + break; +#endif +#ifdef B50 + + case 50: + baud = B50; + break; +#endif +#ifdef B75 + + case 75: + baud = B75; + break; +#endif +#ifdef B110 + + case 110: + baud = B110; + break; +#endif +#ifdef B134 + + case 134: + baud = B134; + break; +#endif +#ifdef B150 + + case 150: + baud = B150; + break; +#endif +#ifdef B200 + + case 200: + baud = B200; + break; +#endif +#ifdef B300 + + case 300: + baud = B300; + break; +#endif +#ifdef B600 + + case 600: + baud = B600; + break; +#endif +#ifdef B1200 + + case 1200: + baud = B1200; + break; +#endif +#ifdef B1800 + + case 1800: + baud = B1800; + break; +#endif +#ifdef B2400 + + case 2400: + baud = B2400; + break; +#endif +#ifdef B4800 + + case 4800: + baud = B4800; + break; +#endif +#ifdef B7200 + + case 7200: + baud = B7200; + break; +#endif +#ifdef B9600 + + case 9600: + baud = B9600; + break; +#endif +#ifdef B14400 + + case 14400: + baud = B14400; + break; +#endif +#ifdef B19200 + + case 19200: + baud = B19200; + break; +#endif +#ifdef B28800 + + case 28800: + baud = B28800; + break; +#endif +#ifdef B57600 + + case 57600: + baud = B57600; + break; +#endif +#ifdef B76800 + + case 76800: + baud = B76800; + break; +#endif +#ifdef B38400 + + case 38400: + baud = B38400; + break; +#endif +#ifdef B115200 + + case 115200: + baud = B115200; + break; +#endif +#ifdef B128000 + + case 128000: + baud = B128000; + break; +#endif +#ifdef B153600 + + case 153600: + baud = B153600; + break; +#endif +#ifdef B230400 + + case 230400: + baud = B230400; + break; +#endif +#ifdef B256000 + + case 256000: + baud = B256000; + break; +#endif +#ifdef B460800 + + case 460800: + baud = B460800; + break; +#endif +#ifdef B576000 + + case 576000: + baud = B576000; + break; +#endif +#ifdef B921600 + + case 921600: + baud = B921600; + break; +#endif +#ifdef B1000000 + + case 1000000: + baud = B1000000; + break; +#endif +#ifdef B1152000 + + case 1152000: + baud = B1152000; + break; +#endif +#ifdef B1500000 + + case 1500000: + baud = B1500000; + break; +#endif +#ifdef B2000000 + + case 2000000: + baud = B2000000; + break; +#endif +#ifdef B2500000 + + case 2500000: + baud = B2500000; + break; +#endif +#ifdef B3000000 + + case 3000000: + baud = B3000000; + break; +#endif +#ifdef B3500000 + + case 3500000: + baud = B3500000; + break; +#endif +#ifdef B4000000 + + case 4000000: + baud = B4000000; + break; +#endif + + default: + custom_baud = true; + } + + return !custom_baud; + +} + + + +Serial::SerialImpl::SerialImpl(const string &port, unsigned long baudrate, + bytesize_t bytesize, + parity_t parity, stopbits_t stopbits, + flowcontrol_t flowcontrol) + : port_(port), fd_(-1), pid(-1), is_open_(false), xonxoff_(false), + rtscts_(false), timeout_(Timeout()), baudrate_(baudrate), byte_time_ns_(0), + parity_(parity), bytesize_(bytesize), stopbits_(stopbits), + flowcontrol_(flowcontrol) { + pthread_mutex_init(&this->read_mutex, NULL); + pthread_mutex_init(&this->write_mutex, NULL); + +} + +Serial::SerialImpl::~SerialImpl() { + close(); + pthread_mutex_destroy(&this->read_mutex); + pthread_mutex_destroy(&this->write_mutex); +} + +bool Serial::SerialImpl::open() { + if (port_.empty()) { + return false; + } + + if (isOpen()) { + return true; + } + + pid = -1; +#ifdef USE_LOCK_FILE + pid = getpid(); + + if (LOCK(port_.c_str(), pid)) { + fprintf(stderr, "Could not lock serial port for exclusive access\n"); + return false; + } + +#endif + + fd_ = ::open(port_.c_str(), + O_RDWR | O_NOCTTY | O_NONBLOCK | O_APPEND | O_NDELAY); + + if (fd_ == -1) { + switch (errno) { + case EINTR: + // Recurse because this is a recoverable error. + return open(); + + case EIO: + +// fprintf(stderr, "I/O error\n"); + + case ENFILE: + +// fprintf(stderr, "File table overflow\n"); + + case EMFILE: + +// fprintf(stderr, "Too many open files\n"); + + default: +// fprintf(stderr, "Default: %d\n", errno); + close(); + return false; + } + } + + termios tio; + + if (!getTermios(&tio)) { + close(); + return false; + } + + set_common_props(&tio); + set_databits(&tio, bytesize_); + set_parity(&tio, parity_); + set_stopbits(&tio, stopbits_); + set_flowcontrol(&tio, flowcontrol_); + + if (!setTermios(&tio)) { + close(); + return false; + } + + if (!setBaudrate(baudrate_)) { + close(); + return false; + } + + // Update byte_time_ based on the new settings. + uint32_t bit_time_ns = 1e9 / baudrate_; + byte_time_ns_ = bit_time_ns * (1 + bytesize_ + parity_ + stopbits_); + + // Compensate for the stopbits_one_point_five enum being equal to int 3, + // and not 1.5. + if (stopbits_ == stopbits_one_point_five) { + byte_time_ns_ += ((1.5 - stopbits_one_point_five) * bit_time_ns); + } + + is_open_ = true; + return true; +} + +Serial::SerialPortError Serial::SerialImpl::getSystemError( + int systemErrorCode) const { + if (systemErrorCode == -1) { + systemErrorCode = errno; + } + + Serial::SerialPortError error; + + switch (systemErrorCode) { + case ENODEV: + error = Serial::DeviceNotFoundError; + break; +#ifdef ENOENT + + case ENOENT: + error = Serial::DeviceNotFoundError; + break; +#endif + + case EACCES: + error = Serial::PermissionError; + break; + + case EBUSY: + error = Serial::PermissionError; + break; + + case EAGAIN: + error = Serial::ResourceError; + break; + + case EIO: + error = Serial::ResourceError; + break; + + case EBADF: + error = Serial::ResourceError; + break; +#ifdef __MACH__ + + case ENXIO: + error = Serial::ResourceError; + break; +#endif +#ifdef EINVAL + + case EINVAL: + error = Serial::UnsupportedOperationError; + break; +#endif +#ifdef ENOIOCTLCMD + + case ENOIOCTLCMD: + error = Serial::UnsupportedOperationError; + break; +#endif +#ifdef ENOTTY + + case ENOTTY: + error = Serial::UnsupportedOperationError; + break; +#endif +#ifdef EPERM + + case EPERM: + error = Serial::PermissionError; + break; +#endif + + default: + error = Serial::UnknownError; + break; + } + + return error; +} + +void Serial::SerialImpl::close() { + if (isOpen()) { + is_open_ = false; + } + + if (fd_ != -1) { + ::close(fd_); + } + +#ifdef USE_LOCK_FILE + + if (pid != -1) { + UNLOCK(port_.c_str(), pid); + } + + pid = -1; +#endif + fd_ = -1; + +} + +bool Serial::SerialImpl::isOpen() const { + return is_open_; +} + +size_t Serial::SerialImpl::available() { + if (!is_open_) { + return 0; + } + + int count = 0; + + if (-1 == ioctl(fd_, TIOCINQ, &count)) { + return 0; + } else { + return static_cast(count); + } +} + +bool Serial::SerialImpl::waitReadable(uint32_t timeout) { + // Setup a select call to block for serial data or a timeout + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(fd_, &readfds); + timespec timeout_ts(timespec_from_ms(timeout)); + int r = pselect(fd_ + 1, &readfds, NULL, NULL, &timeout_ts, NULL); + + if (r < 0) { + // Select was interrupted + if (errno == EINTR) { + return false; + } + + // Otherwise there was some error + return false; + } + + // Timeout occurred + if (r == 0) { + return false; + } + + // This shouldn't happen, if r > 0 our fd has to be in the list! + if (!FD_ISSET(fd_, &readfds)) { + return false; + } + + // Data available to read. + return true; +} + + +int Serial::SerialImpl::waitfordata(size_t data_count, uint32_t timeout, + size_t *returned_size) { + if (!isOpen()) { + return -2; + } + + size_t length = 0; + + if (returned_size == NULL) { + returned_size = (size_t *)&length; + } + + *returned_size = 0; + + if (isOpen()) { + if (ioctl(fd_, FIONREAD, returned_size) == -1) { + return -2; + } + + if (*returned_size >= data_count) { + return 0; + } + } + + fd_set readfds; + /* Initialize the input set */ + FD_ZERO(&readfds); + FD_SET(fd_, &readfds); + + MillisecondTimer total_timeout(timeout); + + while (isOpen()) { + int64_t timeout_remaining_ms = total_timeout.remaining(); + + if ((timeout_remaining_ms <= 0)) { + // Timed out + return -1; + } + + /* Initialize the timeout structure */ + timespec timeout_val(timespec_from_ms(timeout_remaining_ms)); + + /* Do the select */ + int n = pselect(fd_ + 1, &readfds, NULL, NULL, &timeout_val, NULL); + + if (n < 0) { + if (errno == EINTR) { + return -1; + } + + // Otherwise there was some error + return -2; + } else if (n == 0) { + // time out + return -1; + } else { + // data avaliable + if (FD_ISSET(fd_, &readfds)) { + if (ioctl(fd_, FIONREAD, returned_size) < 0) { + return -2; + } + + if (*returned_size >= data_count) { + return 0; + } else { + int remain_timeout = timeout_val.tv_sec * 1000000 + timeout_val.tv_nsec / 1000; + int expect_remain_time = (data_count - *returned_size) * 1000000 * 8 / + baudrate_; + + if (remain_timeout > expect_remain_time) { + usleep(expect_remain_time); + } + } + } else { + usleep(30); + } + } + } + + return -2; +} + + +void Serial::SerialImpl::waitByteTimes(size_t count) { + timespec wait_time = { 0, static_cast(byte_time_ns_ * count)}; + pselect(0, NULL, NULL, NULL, &wait_time, NULL); +} + +size_t Serial::SerialImpl::read(uint8_t *buf, size_t size) { + // If the port is not open, throw + if (!is_open_) { + return 0; + } + + size_t bytes_read = 0; + + // Calculate total timeout in milliseconds t_c + (t_m * N) + long total_timeout_ms = timeout_.read_timeout_constant; + total_timeout_ms += timeout_.read_timeout_multiplier * static_cast(size); + MillisecondTimer total_timeout(total_timeout_ms); + + // Pre-fill buffer with available bytes + { + ssize_t bytes_read_now = ::read(fd_, buf, size); + + if (bytes_read_now > 0) { + bytes_read = bytes_read_now; + } + } + + while (bytes_read < size) { + int64_t timeout_remaining_ms = total_timeout.remaining(); + + if (timeout_remaining_ms <= 0) { + // Timed out + break; + } + + // Timeout for the next select is whichever is less of the remaining + // total read timeout and the inter-byte timeout. + uint32_t timeout = std::min(static_cast(timeout_remaining_ms), + timeout_.inter_byte_timeout); + + // Wait for the device to be readable, and then attempt to read. + if (waitReadable(timeout)) { + // If it's a fixed-length multi-byte read, insert a wait here so that + // we can attempt to grab the whole thing in a single IO call. Skip + // this wait if a non-max inter_byte_timeout is specified. + if (size > 1 && timeout_.inter_byte_timeout == Timeout::max()) { + size_t bytes_available = available(); + + if (bytes_available + bytes_read < size) { + waitByteTimes(size - (bytes_available + bytes_read)); + } + } + + // This should be non-blocking returning only what is available now + // Then returning so that select can block again. + ssize_t bytes_read_now = ::read(fd_, buf + bytes_read, size - bytes_read); + + // read should always return some data as select reported it was + // ready to read when we get to this point. + if (bytes_read_now < 1) { + // Disconnected devices, at least on Linux, show the + // behavior that they are always ready to read immediately + // but reading returns nothing. + continue; + } + + // Update bytes_read + bytes_read += static_cast(bytes_read_now); + + // If bytes_read == size then we have read everything we need + if (bytes_read == size) { + break; + } + + // If bytes_read < size then we have more to read + if (bytes_read < size) { + continue; + } + + // If bytes_read > size then we have over read, which shouldn't happen + if (bytes_read > size) { + break; + } + } + } + + return bytes_read; +} + +size_t Serial::SerialImpl::write(const uint8_t *data, size_t length) { + if (is_open_ == false) { + return 0; + } + + fd_set writefds; + size_t bytes_written = 0; + + // Calculate total timeout in milliseconds t_c + (t_m * N) + long total_timeout_ms = timeout_.write_timeout_constant; + total_timeout_ms += timeout_.write_timeout_multiplier * static_cast + (length); + MillisecondTimer total_timeout(total_timeout_ms); + + bool first_iteration = true; + + while (bytes_written < length) { + int64_t timeout_remaining_ms = total_timeout.remaining(); + + // Only consider the timeout if it's not the first iteration of the loop + // otherwise a timeout of 0 won't be allowed through + if (!first_iteration && (timeout_remaining_ms <= 0)) { + // Timed out + break; + } + + first_iteration = false; + + timespec timeout(timespec_from_ms(timeout_remaining_ms)); + + FD_ZERO(&writefds); + FD_SET(fd_, &writefds); + + // Do the select + int r = pselect(fd_ + 1, NULL, &writefds, NULL, &timeout, NULL); + + // Figure out what happened by looking at select's response 'r' + /** Error **/ + if (r < 0) { + // Select was interrupted, try again + if (errno == EINTR) { + continue; + } + + // Otherwise there was some error + continue; + } + + /** Timeout **/ + if (r == 0) { + break; + } + + /** Port ready to write **/ + if (r > 0) { + // Make sure our file descriptor is in the ready to write list + if (FD_ISSET(fd_, &writefds)) { + // This will write some + ssize_t bytes_written_now = ::write(fd_, data + bytes_written, + length - bytes_written); + + // write should always return some data as select reported it was + // ready to write when we get to this point. + if (bytes_written_now < 1) { + // Disconnected devices, at least on Linux, show the + // behavior that they are always ready to write immediately + // but writing returns nothing. + continue; + } + + // Update bytes_written + bytes_written += static_cast(bytes_written_now); + + // If bytes_written == size then we have written everything we need to + if (bytes_written == length) { + break; + } + + // If bytes_written < size then we have more to write + if (bytes_written < length) { + continue; + } + + // If bytes_written > size then we have over written, which shouldn't happen + if (bytes_written > length) { + break; + } + } + + // This shouldn't happen, if r > 0 our fd has to be in the list! + break; + //THROW (IOException, "select reports ready to write, but our fd isn't in the list, this shouldn't happen!"); + } + } + + return bytes_written; +} + +void Serial::SerialImpl::setPort(const string &port) { + port_ = port; +} + +string Serial::SerialImpl::getPort() const { + return port_; +} + +void Serial::SerialImpl::setTimeout(serial::Timeout &timeout) { + timeout_ = timeout; +} + +serial::Timeout Serial::SerialImpl::getTimeout() const { + return timeout_; +} + +bool Serial::SerialImpl::setBaudrate(unsigned long baudrate) { + + if (fd_ == -1) { + return false; + } + + baudrate_ = baudrate; + // OS X support +#if defined(MAC_OS_X_VERSION_10_4) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_4) + // Starting with Tiger, the IOSSIOSPEED ioctl can be used to set arbitrary baud rates + // other than those specified by POSIX. The driver for the underlying serial hardware + // ultimately determines which baud rates can be used. This ioctl sets both the input + // and output speed. + speed_t new_baud = static_cast(baudrate); + + if (-1 == ioctl(fd_, IOSSIOSPEED, &new_baud, 1)) { + return false; + } + + // Linux Support +#elif defined(__linux__) && defined (TIOCSSERIAL) + speed_t baud; + bool standard_baud = is_standardbaudrate(baudrate, baud); + + if (!standard_baud) { + return setCustomBaudRate(baudrate); + } else { + return setStandardBaudRate(baud); + } + +#else + return false; +#endif + +} + +unsigned long Serial::SerialImpl::getBaudrate() const { + return baudrate_; +} + + +bool Serial::SerialImpl::setStandardBaudRate(speed_t baudrate) { +#ifdef __linux__ + // try to clear custom baud rate, using termios v2 + struct termios2 tio2; + + if (::ioctl(fd_, TCGETS2, &tio2) != -1) { + if (tio2.c_cflag & BOTHER) { + tio2.c_cflag &= ~BOTHER; + tio2.c_cflag |= CBAUD; + ::ioctl(fd_, TCSETS2, &tio2); + } + } + + // try to clear custom baud rate, using serial_struct (old way) + struct serial_struct serial; + ::memset(&serial, 0, sizeof(serial)); + + if (::ioctl(fd_, TIOCGSERIAL, &serial) != -1) { + if (serial.flags & ASYNC_SPD_CUST) { + serial.flags &= ~ASYNC_SPD_CUST; + serial.custom_divisor = 0; + // we don't check on errors because a driver can has not this feature + ::ioctl(fd_, TIOCSSERIAL, &serial); + } + } + +#endif + + termios tio; + + if (!getTermios(&tio)) { + return false; + } + +#ifdef _BSD_SOURCE + + if (::cfsetspeed(&tio, baudrate) < 0) { + return false; + } + +#else + + if (::cfsetispeed(&tio, baudrate) < 0) { + return false; + } + + if (::cfsetospeed(&tio, baudrate) < 0) { + return false; + } + +#endif + return setTermios(&tio); +} + + +bool Serial::SerialImpl::setCustomBaudRate(unsigned long baudrate) { + struct termios2 tio2; + + if (::ioctl(fd_, TCGETS2, &tio2) != -1) { + tio2.c_cflag &= ~CBAUD; + tio2.c_cflag |= BOTHER; + + tio2.c_ispeed = baudrate; + tio2.c_ospeed = baudrate; + + tcflush(fd_, TCIFLUSH); + + if (fcntl(fd_, F_SETFL, FNDELAY)) { + return false; + } + + /*struct flock file_lock; + file_lock.l_type = F_WRLCK; + file_lock.l_whence = SEEK_SET; + file_lock.l_start = 0; + file_lock.l_len = 0; + file_lock.l_pid = getpid(); + if (fcntl(fd_, F_SETLK, &file_lock) != 0) { + return false; + }*/ + + + + if (::ioctl(fd_, TCSETS2, &tio2) != -1 && ::ioctl(fd_, TCGETS2, &tio2) != -1) { + return true; + } + } + + struct serial_struct serial; + + if (::ioctl(fd_, TIOCGSERIAL, &serial) == -1) { + return false; + } + + serial.flags &= ~ASYNC_SPD_MASK; + serial.flags |= (ASYNC_SPD_CUST /* | ASYNC_LOW_LATENCY*/); + serial.custom_divisor = serial.baud_base / baudrate; + + if (serial.custom_divisor == 0) { + return false; + } + + if (serial.custom_divisor * baudrate != serial.baud_base) { + } + + if (::ioctl(fd_, TIOCSSERIAL, &serial) == -1) { + return false; + } + + return setStandardBaudRate(B38400); +} + + +bool Serial::SerialImpl::setBytesize(serial::bytesize_t bytesize) { + termios tio; + + if (!getTermios(&tio)) { + return false; + } + + bytesize_ = bytesize; + set_databits(&tio, bytesize); + + return setTermios(&tio); +} + +serial::bytesize_t Serial::SerialImpl::getBytesize() const { + return bytesize_; +} + +bool Serial::SerialImpl::setParity(serial::parity_t parity) { + termios tio; + + if (!getTermios(&tio)) { + return false; + } + + parity_ = parity; + set_parity(&tio, parity); + + return setTermios(&tio); +} + +serial::parity_t Serial::SerialImpl::getParity() const { + return parity_; +} + +bool Serial::SerialImpl::setStopbits(serial::stopbits_t stopbits) { + termios tio; + + if (!getTermios(&tio)) { + return false; + } + + stopbits_ = stopbits; + set_stopbits(&tio, stopbits); + + return setTermios(&tio); +} + +serial::stopbits_t Serial::SerialImpl::getStopbits() const { + return stopbits_; +} + +bool Serial::SerialImpl::setFlowcontrol(serial::flowcontrol_t flowcontrol) { + termios tio; + + if (!getTermios(&tio)) { + return false; + } + + flowcontrol_ = flowcontrol; + set_flowcontrol(&tio, flowcontrol); + + return setTermios(&tio); +} + +serial::flowcontrol_t Serial::SerialImpl::getFlowcontrol() const { + return flowcontrol_; +} + + +bool Serial::SerialImpl::setTermios(const termios *tio) { + + tcflush(fd_, TCIFLUSH); + + if (fcntl(fd_, F_SETFL, FNDELAY)) { + return false; + } + + if (::tcsetattr(fd_, TCSANOW, tio) == -1) { + return false; + } + + return true; +} + +bool Serial::SerialImpl::getTermios(termios *tio) { + ::memset(tio, 0, sizeof(termios)); + + if (::tcgetattr(fd_, tio) == -1) { + return false; + } + + return true; +} + +void Serial::SerialImpl::flush() { + if (is_open_ == false) { + return; + } + +#if !defined(__ANDROID__) + tcdrain(fd_); +#endif +} + +void Serial::SerialImpl::flushInput() { + if (is_open_ == false) { + return; + } + + tcflush(fd_, TCIFLUSH); +} + +void Serial::SerialImpl::flushOutput() { + if (is_open_ == false) { + return; + } + + tcflush(fd_, TCOFLUSH); +} + +void Serial::SerialImpl::sendBreak(int duration) { + if (is_open_ == false) { + return; + } + + tcsendbreak(fd_, static_cast(duration / 4)); +} + +bool Serial::SerialImpl::setBreak(bool level) { + if (is_open_ == false) { + return false; + } + + if (level) { + if (-1 == ioctl(fd_, TIOCSBRK)) { + return false; + } + } else { + if (-1 == ioctl(fd_, TIOCCBRK)) { + return false; + } + } + + return true; +} + +bool Serial::SerialImpl::setRTS(bool level) { + if (is_open_ == false) { + return false; + } + + int command = TIOCM_RTS; + + if (level) { + if (-1 == ioctl(fd_, TIOCMBIS, &command)) { + return false; + } + } else { + if (-1 == ioctl(fd_, TIOCMBIC, &command)) { + return false; + } + } + + return true; +} + +bool Serial::SerialImpl::setDTR(bool level) { + if (is_open_ == false) { + return false; + } + + int command = TIOCM_DTR; + + if (level) { + if (-1 == ioctl(fd_, TIOCMBIS, &command)) { + return false; + } + } else { + if (-1 == ioctl(fd_, TIOCMBIC, &command)) { + return false; + } + } + + return true; +} + +bool Serial::SerialImpl::waitForChange() { +#ifndef TIOCMIWAIT + + while (is_open_ == true) { + + int status; + + if (-1 == ioctl(fd_, TIOCMGET, &status)) { + return false; + } else { + if (0 != (status & TIOCM_CTS) + || 0 != (status & TIOCM_DSR) + || 0 != (status & TIOCM_RI) + || 0 != (status & TIOCM_CD)) { + return true; + } + } + + usleep(1000); + } + + return false; +#else + int command = (TIOCM_CD | TIOCM_DSR | TIOCM_RI | TIOCM_CTS); + + if (-1 == ioctl(fd_, TIOCMIWAIT, &command)) { + return false; + } + + return true; +#endif +} + +bool Serial::SerialImpl::getCTS() { + if (is_open_ == false) { + return false; + } + + int status; + + if (-1 == ioctl(fd_, TIOCMGET, &status)) { + return false; + } else { + return 0 != (status & TIOCM_CTS); + } +} + +bool Serial::SerialImpl::getDSR() { + if (is_open_ == false) { + return false; + } + + int status; + + if (-1 == ioctl(fd_, TIOCMGET, &status)) { + return false; + } else { + return 0 != (status & TIOCM_DSR); + } +} + +bool Serial::SerialImpl::getRI() { + if (is_open_ == false) { + return false; + } + + int status; + + if (-1 == ioctl(fd_, TIOCMGET, &status)) { + return false; + } else { + return 0 != (status & TIOCM_RI); + } +} + +bool Serial::SerialImpl::getCD() { + if (is_open_ == false) { + return false; + } + + int status; + + if (-1 == ioctl(fd_, TIOCMGET, &status)) { + return false; + } else { + return 0 != (status & TIOCM_CD); + } +} + +uint32_t Serial::SerialImpl::getByteTime() { + return byte_time_ns_; +} + +int Serial::SerialImpl::readLock() { + int result = pthread_mutex_lock(&this->read_mutex); + return result; +} + +int Serial::SerialImpl::readUnlock() { + int result = pthread_mutex_unlock(&this->read_mutex); + return result; +} + +int Serial::SerialImpl::writeLock() { + int result = pthread_mutex_lock(&this->write_mutex); + return result; +} + +int Serial::SerialImpl::writeUnlock() { + int result = pthread_mutex_unlock(&this->write_mutex); + return result; +} +}//namespace serial +}//namespace core +}//namespace ydlidar +#endif // !defined(_WIN32) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.h new file mode 100644 index 0000000..6f58dc9 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/unix/unix_serial.h @@ -0,0 +1,162 @@ +#if !defined(_WIN32) + +#ifndef SERIAL_IMPL_UNIX_H +#define SERIAL_IMPL_UNIX_H + +#include +#include +#include +#include + +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) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/CMakeLists.txt new file mode 100644 index 0000000..c8afd28 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/CMakeLists.txt @@ -0,0 +1,5 @@ +aux_include_directory(. HDRS) +aux_src_directory(. SRCS) +add_to_ydlidar_headers(${HDRS}) +add_to_ydlidar_sources(${SRCS}) + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/list_ports_win.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/list_ports_win.cpp new file mode 100644 index 0000000..05900a5 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/list_ports_win.cpp @@ -0,0 +1,196 @@ +#if defined(_WIN32) +#pragma comment(lib, "setupapi.lib") +#ifdef QT_VERSION +#include +#if (QT_VERSION <= QT_VERSION_CHECK(5,9,0)) +#undef UNICODE +#endif +#else +#undef UNICODE +#endif +#include +#include +#include +#include +#include +#include +#include + +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 +serial::list_ports() { + vector 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) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win.h new file mode 100644 index 0000000..9e6506d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win.h @@ -0,0 +1,10 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.cpp new file mode 100644 index 0000000..4cce5cb --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.cpp @@ -0,0 +1,1077 @@ +#if defined(_WIN32) +#include "win_serial.h" + +namespace ydlidar { +namespace core { +namespace serial { + +static inline void set_common_props(DCB *dcb) { + dcb->fBinary = TRUE; + dcb->fAbortOnError = FALSE; + dcb->fNull = FALSE; + dcb->fErrorChar = FALSE; + + if (dcb->fDtrControl == DTR_CONTROL_HANDSHAKE) { + dcb->fDtrControl = DTR_CONTROL_DISABLE; + } + + if (dcb->fRtsControl != RTS_CONTROL_HANDSHAKE) { + dcb->fRtsControl = RTS_CONTROL_DISABLE; + } +} + +static inline void set_baudrate(DCB *dcb, unsigned long baudrate) { + // setup baud rate + switch (baudrate) { +#ifdef CBR_0 + + case 0: + dcb->BaudRate = CBR_0; + break; +#endif +#ifdef CBR_50 + + case 50: + dcb->BaudRate = CBR_50; + break; +#endif +#ifdef CBR_75 + + case 75: + dcb->BaudRate = CBR_75; + break; +#endif +#ifdef CBR_110 + + case 110: + dcb->BaudRate = CBR_110; + break; +#endif +#ifdef CBR_134 + + case 134: + dcb->BaudRate = CBR_134; + break; +#endif +#ifdef CBR_150 + + case 150: + dcb->BaudRate = CBR_150; + break; +#endif +#ifdef CBR_200 + + case 200: + dcb->BaudRate = CBR_200; + break; +#endif +#ifdef CBR_300 + + case 300: + dcb->BaudRate = CBR_300; + break; +#endif +#ifdef CBR_600 + + case 600: + dcb->BaudRate = CBR_600; + break; +#endif +#ifdef CBR_1200 + + case 1200: + dcb->BaudRate = CBR_1200; + break; +#endif +#ifdef CBR_1800 + + case 1800: + dcb->BaudRate = CBR_1800; + break; +#endif +#ifdef CBR_2400 + + case 2400: + dcb->BaudRate = CBR_2400; + break; +#endif +#ifdef CBR_4800 + + case 4800: + dcb->BaudRate = CBR_4800; + break; +#endif +#ifdef CBR_7200 + + case 7200: + dcb->BaudRate = CBR_7200; + break; +#endif +#ifdef CBR_9600 + + case 9600: + dcb->BaudRate = CBR_9600; + break; +#endif +#ifdef CBR_14400 + + case 14400: + dcb->BaudRate = CBR_14400; + break; +#endif +#ifdef CBR_19200 + + case 19200: + dcb->BaudRate = CBR_19200; + break; +#endif +#ifdef CBR_28800 + + case 28800: + dcb->BaudRate = CBR_28800; + break; +#endif +#ifdef CBR_57600 + + case 57600: + dcb->BaudRate = CBR_57600; + break; +#endif +#ifdef CBR_76800 + + case 76800: + dcb->BaudRate = CBR_76800; + break; +#endif +#ifdef CBR_38400 + + case 38400: + dcb->BaudRate = CBR_38400; + break; +#endif +#ifdef CBR_115200 + + case 115200: + dcb->BaudRate = CBR_115200; + break; +#endif +#ifdef CBR_128000 + + case 128000: + dcb->BaudRate = CBR_128000; + break; +#endif +#ifdef CBR_153600 + + case 153600: + dcb->BaudRate = CBR_153600; + break; +#endif +#ifdef CBR_230400 + + case 230400: + dcb->BaudRate = CBR_230400; + break; +#endif +#ifdef CBR_256000 + + case 256000: + dcb->BaudRate = CBR_256000; + break; +#endif +#ifdef CBR_460800 + + case 460800: + dcb->BaudRate = CBR_460800; + break; +#endif +#ifdef CBR_921600 + + case 921600: + dcb->BaudRate = CBR_921600; + break; +#endif + + default: + // Try to blindly assign it + dcb->BaudRate = baudrate; + } +} + +static inline void set_databits(DCB *dcb, serial::bytesize_t bytesize) { + switch (bytesize) { + case serial::fivebits: + dcb->ByteSize = 5; + break; + + case serial::sixbits: + dcb->ByteSize = 6; + break; + + case serial::sevenbits: + dcb->ByteSize = 7; + break; + + case serial::eightbits: + dcb->ByteSize = 8; + break; + + default: + dcb->ByteSize = 8; + break; + } +} + +static inline void set_parity(DCB *dcb, serial::parity_t parity) { + dcb->fParity = TRUE; + + switch (parity) { + case serial::parity_none: + dcb->Parity = NOPARITY; + dcb->fParity = FALSE; + break; + + case serial::parity_odd: + dcb->Parity = ODDPARITY; + break; + + case serial::parity_even: + dcb->Parity = EVENPARITY; + break; + + case serial::parity_mark: + dcb->Parity = MARKPARITY; + break; + + case serial::parity_space: + dcb->Parity = SPACEPARITY; + break; + + default: + dcb->Parity = NOPARITY; + dcb->fParity = FALSE; + break; + } +} + +static inline void set_stopbits(DCB *dcb, serial::stopbits_t stopbits) { + switch (stopbits) { + case serial::stopbits_one: + dcb->StopBits = ONESTOPBIT; + break; + + case serial::stopbits_one_point_five: + dcb->StopBits = ONE5STOPBITS; + break; + + case serial::stopbits_two: + dcb->StopBits = TWOSTOPBITS; + break; + + default: + dcb->StopBits = ONESTOPBIT; + break; + } +} + +static inline void set_flowcontrol(DCB *dcb, + serial::flowcontrol_t flowcontrol) { + dcb->fInX = FALSE; + dcb->fOutX = FALSE; + dcb->fOutxCtsFlow = FALSE; + + if (dcb->fRtsControl == RTS_CONTROL_HANDSHAKE) { + dcb->fRtsControl = RTS_CONTROL_DISABLE; + } + + switch (flowcontrol) { + case serial::flowcontrol_none: + break; + + case serial::flowcontrol_software: + dcb->fInX = TRUE; + dcb->fOutX = TRUE; + break; + + case serial::flowcontrol_hardware: + dcb->fOutxCtsFlow = TRUE; + dcb->fRtsControl = RTS_CONTROL_HANDSHAKE; + break; + + default: + break; + } +} + +inline wstring _prefix_port_if_needed(const wstring &input) { + static wstring windows_com_port_prefix = L"\\\\.\\"; + + if (input.compare(windows_com_port_prefix) != 0) { + return windows_com_port_prefix + input; + } + + return input; +} + +Serial::SerialImpl::SerialImpl(const string &port, unsigned long baudrate, + bytesize_t bytesize, + parity_t parity, stopbits_t stopbits, + flowcontrol_t flowcontrol) + : port_(port.begin(), port.end()), fd_(INVALID_HANDLE_VALUE), is_open_(false), + baudrate_(baudrate), parity_(parity), + bytesize_(bytesize), stopbits_(stopbits), flowcontrol_(flowcontrol) { + if (port_.empty() == false) { + open(); + } + + read_mutex = CreateMutex(NULL, false, NULL); + write_mutex = CreateMutex(NULL, false, NULL); + memset(&_wait_o, 0, sizeof(_wait_o)); + + _wait_o.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); +} + +Serial::SerialImpl::~SerialImpl() { + this->close(); + CloseHandle(_wait_o.hEvent); + CloseHandle(read_mutex); + CloseHandle(write_mutex); +} + +bool Serial::SerialImpl::open() { + if (port_.empty()) { + return false; + } + + if (is_open_ == true) { + return true; + } + + DWORD desiredAccess = 0; + originalEventMask = 0; + + + desiredAccess |= GENERIC_READ; + originalEventMask = EV_RXCHAR | EV_ERR ; + desiredAccess |= GENERIC_WRITE; + + wstring port_with_prefix = _prefix_port_if_needed(port_); + LPCWSTR lp_port = port_with_prefix.c_str(); + fd_ = CreateFile(lp_port, + desiredAccess, + 0, + nullptr, + OPEN_EXISTING, + FILE_ATTRIBUTE_NORMAL | + FILE_FLAG_OVERLAPPED,//FILE_FLAG_OVERLAPPED,//FILE_ATTRIBUTE_NORMAL + nullptr); + + if (fd_ == INVALID_HANDLE_VALUE) { + DWORD errno_ = GetLastError(); + + switch (errno_) { + case ERROR_FILE_NOT_FOUND: + default: + return false; + + } + } + + + + if (reconfigurePort()) { + is_open_ = true; + return true; + } + + ::CloseHandle(fd_); + return false; + +} + +bool Serial::SerialImpl::reconfigurePort() { + if (fd_ == INVALID_HANDLE_VALUE) { + // Can only operate on a valid file descriptor + return false; + } + + if (!SetupComm(fd_, DEFAULT_RX_BUFFER_SIZE, DEFAULT_TX_BUFFER_SIZE)) { + return false; + } + + DCB dcb; + + if (!getDcb(&dcb)) { + return false; + } + + set_common_props(&dcb); + set_baudrate(&dcb, baudrate_); + set_databits(&dcb, bytesize_); + set_parity(&dcb, parity_); + set_stopbits(&dcb, stopbits_); + set_flowcontrol(&dcb, flowcontrol_); + + if (!setDcb(&dcb)) { + return false; + } + + if (!::GetCommTimeouts(fd_, &restoredCommTimeouts)) { + return false; + } + + // Update byte_time_ based on the new settings. + uint32_t bit_time_ns = 1e9 / dcb.BaudRate; + + ::ZeroMemory(¤tCommTimeouts, sizeof(currentCommTimeouts)); + DWORD serialBitsPerByte = dcb.ByteSize + 1; + serialBitsPerByte += (dcb.Parity == NOPARITY) ? 0 : 1; + serialBitsPerByte += (dcb.StopBits == ONESTOPBIT) ? 1 : 2; + + + DWORD msPerByte = (dcb.BaudRate > 0) ? ((1000 * serialBitsPerByte + dcb.BaudRate + - 1) / + dcb.BaudRate) : 1; + currentCommTimeouts.ReadIntervalTimeout = + msPerByte; //最小化串联端口数据包连接读取的机会// Minimize chance of concatenating of separate serial port packets on read + currentCommTimeouts.ReadTotalTimeoutMultiplier = + 0; //当使用大的读取缓冲区时，不允许大的读取超时// Do not allow big read timeout when big read buffer used + currentCommTimeouts.ReadTotalTimeoutConstant = + 2000; //总读取超时（读取循环的周期）// Total read timeout (period of read loop) + currentCommTimeouts.WriteTotalTimeoutConstant = + 2000; //写超时的常量部分// Const part of write timeout + currentCommTimeouts.WriteTotalTimeoutMultiplier = + msPerByte; //写入超时的变量部分（每个字节）// Variable part of write timeout (per byte) + + byte_time_ns_ = bit_time_ns * msPerByte; + + if (!::SetCommTimeouts(fd_, ¤tCommTimeouts)) { + return false; + } + + if (!::SetCommMask(fd_, originalEventMask)) { + return false; + } + + if (!PurgeComm(fd_, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | + PURGE_RXCLEAR)) { + return false; + } + + + ::ZeroMemory(&communicationOverlapped, sizeof(communicationOverlapped)); + + if (!(originalEventMask & EV_RXCHAR) && + !::WaitCommEvent(fd_, &triggeredEventMask, &communicationOverlapped)) { + return false; + } + + return true; + +} +Serial::SerialPortError Serial::SerialImpl::getSystemError( + int systemErrorCode) const { + if (systemErrorCode == -1) { + systemErrorCode = GetLastError(); + } + + Serial::SerialPortError error; + + switch (systemErrorCode) { + case ERROR_SUCCESS: + error = Serial::NoError; + break; + + case ERROR_IO_PENDING: + error = Serial::NoError; + break; + + case ERROR_MORE_DATA: + error = Serial::NoError; + break; + + case ERROR_FILE_NOT_FOUND: + error = Serial::DeviceNotFoundError; + break; + + case ERROR_PATH_NOT_FOUND: + error = Serial::DeviceNotFoundError; + break; + + case ERROR_INVALID_NAME: + error = Serial::DeviceNotFoundError; + break; + + case ERROR_ACCESS_DENIED: + error = Serial::PermissionError; + break; + + case ERROR_INVALID_HANDLE: + error = Serial::ResourceError; + break; + + case ERROR_INVALID_PARAMETER: + error = Serial::UnsupportedOperationError; + break; + + case ERROR_BAD_COMMAND: + error = Serial::ResourceError; + break; + + case ERROR_DEVICE_REMOVED: + error = Serial::ResourceError; + break; + + case ERROR_OPERATION_ABORTED: + error = Serial::ResourceError; + break; + + case WAIT_TIMEOUT: + error = Serial::TimeoutError; + break; + + default: + error = Serial::UnknownError; + break; + } + + return error; +} + +void Serial::SerialImpl::close() { + ResetEvent(_wait_o.hEvent); + + if (is_open_ == true) { + if (fd_ != INVALID_HANDLE_VALUE) { + ::CancelIo(fd_); + int ret; + ret = CloseHandle(fd_); + } + + fd_ = INVALID_HANDLE_VALUE; + is_open_ = false; + } +} + +bool Serial::SerialImpl::isOpen() const { + return is_open_; +} + +size_t Serial::SerialImpl::available() { + if (!is_open_) { + return 0; + } + + COMSTAT cs; + DWORD error; + + if (ClearCommError(fd_, &error, &cs) && error > 0) { + PurgeComm(fd_, PURGE_RXABORT | PURGE_RXCLEAR); + return 0; + } + + return static_cast(cs.cbInQue); +} + +bool Serial::SerialImpl::waitReadable(uint32_t /*timeout*/) { + return false; +} + +void Serial::SerialImpl::waitByteTimes(size_t /*count*/) { + return ; +} + +int Serial::SerialImpl::waitfordata(size_t data_count, uint32_t timeout, + size_t *returned_size) { + if (!is_open_) { + return 0; + } + + size_t length = 0; + + if (returned_size == NULL) { + returned_size = (size_t *)&length; + } + + *returned_size = 0; + + if (is_open_) { + size_t queue_remaining = available(); + + if (queue_remaining >= data_count) { + *returned_size = queue_remaining; + return 0; + } + } + + COMSTAT stat; + DWORD error; + DWORD msk, lengths; + + while (is_open_) { + msk = 0; + SetCommMask(fd_, EV_RXCHAR | EV_ERR); + + if (!WaitCommEvent(fd_, &msk, &_wait_o)) { + if (GetLastError() == ERROR_IO_PENDING) { + + if (WaitForSingleObject(_wait_o.hEvent, timeout) == WAIT_TIMEOUT) { + *returned_size = 0; + return -1; + } + + GetOverlappedResult(fd_, &_wait_o, &lengths, TRUE); + + ::ResetEvent(_wait_o.hEvent); + } else { + ClearCommError(fd_, &error, &stat); + *returned_size = stat.cbInQue; + return -2; + } + } + + if (msk & EV_ERR) { + // FIXME: may cause problem here + ClearCommError(fd_, &error, &stat); + } + + if (msk & EV_RXCHAR) { + ClearCommError(fd_, &error, &stat); + + if (stat.cbInQue >= data_count) { + *returned_size = stat.cbInQue; + return 0; + } + } + } + + *returned_size = 0; + return -2; +} + + +size_t Serial::SerialImpl::read(uint8_t *buf, size_t size) { + if (!is_open_) { + return 0; + } + + DWORD bytes_read; + ::ZeroMemory(&readCompletionOverlapped, sizeof(readCompletionOverlapped)); + + if (!ReadFile(fd_, buf, static_cast(size), &bytes_read, + &readCompletionOverlapped)) { + if (GetLastError() == ERROR_IO_PENDING) { + DWORD dwWait =::WaitForSingleObject(fd_, INFINITE); + + if (dwWait != WAIT_OBJECT_0) { + if (!GetOverlappedResult(fd_, NULL, &bytes_read, TRUE)) { + if (GetLastError() != ERROR_IO_INCOMPLETE) { + return 0; + } + } + } + } + + return 0; + } + + return (int)(bytes_read); +} + +size_t Serial::SerialImpl::write(const uint8_t *data, size_t length) { + if (is_open_ == false) { + return 0; + } + + if (data == NULL || length == 0) { + return 0; + } + + DWORD error; + + if (ClearCommError(fd_, &error, NULL) && error > 0) { + PurgeComm(fd_, PURGE_TXABORT | PURGE_TXCLEAR); + } + + DWORD bytes_written; + ::ZeroMemory(&writeCompletionOverlapped, sizeof(writeCompletionOverlapped)); + + if (!::WriteFile(fd_, data, static_cast(length), &bytes_written, + &writeCompletionOverlapped)) { + return 0; + } + + return (int)(bytes_written); +} + +void Serial::SerialImpl::setPort(const string &port) { + port_ = wstring(port.begin(), port.end()); +} + +string Serial::SerialImpl::getPort() const { + return string(port_.begin(), port_.end()); +} + +void Serial::SerialImpl::setTimeout(serial::Timeout &timeout) { + timeout_ = timeout; + + if (fd_ == INVALID_HANDLE_VALUE) { + return; + } + + COMMTIMEOUTS currentTimeouts; + + if (!::GetCommTimeouts(fd_, ¤tTimeouts)) { + return; + } + + // Setup timeouts + ::ZeroMemory(¤tCommTimeouts, sizeof(currentCommTimeouts)); + currentCommTimeouts.ReadIntervalTimeout = + currentTimeouts.ReadIntervalTimeout;//MAXWORD; + currentCommTimeouts.ReadTotalTimeoutConstant = timeout_.read_timeout_constant; + currentCommTimeouts.ReadTotalTimeoutMultiplier = + timeout_.read_timeout_multiplier; + currentCommTimeouts.WriteTotalTimeoutConstant = timeout_.write_timeout_constant; + currentCommTimeouts.WriteTotalTimeoutMultiplier = + currentTimeouts.WriteTotalTimeoutMultiplier;//timeout_.write_timeout_multiplier; + + if (!SetCommTimeouts(fd_, ¤tCommTimeouts)) { + return; + } + +} + +serial::Timeout Serial::SerialImpl::getTimeout() const { + return timeout_; +} + + +bool Serial::SerialImpl::setDcb(DCB *dcb) { + if (!::SetCommState(fd_, dcb)) { + return false; + } + + return true; +} + +bool Serial::SerialImpl::getDcb(DCB *dcb) { + ::ZeroMemory(dcb, sizeof(DCB)); + dcb->DCBlength = sizeof(DCB); + + if (!::GetCommState(fd_, dcb)) { + return false; + } + + return true; +} + + +bool Serial::SerialImpl::setBaudrate(unsigned long baudrate) { + DCB dcb; + + if (!getDcb(&dcb)) { + return false; + } + + baudrate_ = baudrate; + set_baudrate(&dcb, baudrate); + + return setDcb(&dcb); +} + +unsigned long Serial::SerialImpl::getBaudrate() const { + return baudrate_; +} + +bool Serial::SerialImpl::setBytesize(serial::bytesize_t bytesize) { + + DCB dcb; + + if (!getDcb(&dcb)) { + return false; + } + + bytesize_ = bytesize; + set_databits(&dcb, bytesize); + + return setDcb(&dcb); +} + +serial::bytesize_t Serial::SerialImpl::getBytesize() const { + return bytesize_; +} + +bool Serial::SerialImpl::setParity(serial::parity_t parity) { + + DCB dcb; + + if (!getDcb(&dcb)) { + return false; + } + + parity_ = parity; + set_parity(&dcb, parity); + + return setDcb(&dcb); +} + +serial::parity_t Serial::SerialImpl::getParity() const { + return parity_; +} + +bool Serial::SerialImpl::setStopbits(serial::stopbits_t stopbits) { + + DCB dcb; + + if (!getDcb(&dcb)) { + return false; + } + + stopbits_ = stopbits; + set_stopbits(&dcb, stopbits); + + return setDcb(&dcb); +} + +serial::stopbits_t Serial::SerialImpl::getStopbits() const { + return stopbits_; +} + +bool Serial::SerialImpl::setFlowcontrol(serial::flowcontrol_t flowcontrol) { + + DCB dcb; + + if (!getDcb(&dcb)) { + return false; + } + + flowcontrol_ = flowcontrol; + set_flowcontrol(&dcb, flowcontrol); + + return setDcb(&dcb); +} + +serial::flowcontrol_t Serial::SerialImpl::getFlowcontrol() const { + return flowcontrol_; +} + +void Serial::SerialImpl::flush() { + if (is_open_ == false) { + return; + } + + //FlushFileBuffers (fd_); + PurgeComm(fd_, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); +} + +void Serial::SerialImpl::flushInput() { + return; +} + +void Serial::SerialImpl::flushOutput() { + return; +} + +void Serial::SerialImpl::sendBreak(int duration) { + if (!setBreak(true)) { + return ; + } + + ::Sleep(duration); + + if (!setBreak(false)) { + return ; + } + +} + +bool Serial::SerialImpl::setBreak(bool level) { + if (is_open_ == false) { + return false; + } + + if (level) { + //EscapeCommFunction (fd_, SETBREAK); + //::SetCommBreak(fd_); + if (::SetCommBreak(fd_) == FALSE) { + return false; + } + } else { + //::ClearCommBreak(fd_); + //EscapeCommFunction (fd_, CLRBREAK); + if (::ClearCommBreak(fd_) == FALSE) { + return false; + } + } + + return true; +} + +bool Serial::SerialImpl::setRTS(bool level) { + if (is_open_ == false) { + return false; + } + + if (level) { + //EscapeCommFunction (fd_, SETRTS); + if (EscapeCommFunction(fd_, SETRTS) == FALSE) { + return false; + } + } else { + //EscapeCommFunction (fd_, CLRRTS); + if (EscapeCommFunction(fd_, CLRRTS) == FALSE) { + return false; + } + } + + return true; +} + +bool Serial::SerialImpl::setDTR(bool level) { + if (is_open_ == false) { + return false; + } + + if (level) { + //EscapeCommFunction (fd_, SETDTR); + if (EscapeCommFunction(fd_, SETDTR) == FALSE) { + return false; + } + } else { + //EscapeCommFunction (fd_, CLRDTR); + if (EscapeCommFunction(fd_, CLRDTR) == FALSE) { + return false; + } + } + + DCB dcb; + + if (!getDcb(&dcb)) { + return false; + } + + dcb.fDtrControl = level ? DTR_CONTROL_ENABLE : DTR_CONTROL_DISABLE; + return setDcb(&dcb); +} + +bool Serial::SerialImpl::waitForChange() { + if (is_open_ == false) { + return false; + } + + DWORD dwCommEvent; + + if (!SetCommMask(fd_, EV_CTS | EV_DSR | EV_RING | EV_RLSD)) { + // Error setting communications mask + return false; + } + + if (!WaitCommEvent(fd_, &dwCommEvent, NULL)) { + // An error occurred waiting for the event. + return false; + } else { + // Event has occurred. + return true; + } +} + +bool Serial::SerialImpl::getCTS() { + if (is_open_ == false) { + return false; + } + + DWORD dwModemStatus; + + if (!GetCommModemStatus(fd_, &dwModemStatus)) { + return false; + } + + return (MS_CTS_ON & dwModemStatus) != 0; +} + +bool Serial::SerialImpl::getDSR() { + if (is_open_ == false) { + return false; + } + + DWORD dwModemStatus; + + if (!GetCommModemStatus(fd_, &dwModemStatus)) { + return false; + } + + return (MS_DSR_ON & dwModemStatus) != 0; +} + +bool Serial::SerialImpl::getRI() { + if (is_open_ == false) { + return false; + } + + DWORD dwModemStatus; + + if (!GetCommModemStatus(fd_, &dwModemStatus)) { + return false; + } + + return (MS_RING_ON & dwModemStatus) != 0; +} + +bool Serial::SerialImpl::getCD() { + if (is_open_ == false) { + return false; + } + + DWORD dwModemStatus; + + if (!GetCommModemStatus(fd_, &dwModemStatus)) { + // Error in GetCommModemStatus; + return false; + } + + return (MS_RLSD_ON & dwModemStatus) != 0; +} + +uint32_t Serial::SerialImpl::getByteTime() { + return byte_time_ns_; +} + + +int Serial::SerialImpl::readLock() { + if (WaitForSingleObject(read_mutex, INFINITE) != WAIT_OBJECT_0) { + return 1; + } + + return 0; +} + +int Serial::SerialImpl::readUnlock() { + if (!ReleaseMutex(read_mutex)) { + return 1; + } + + return 0; +} + +int Serial::SerialImpl::writeLock() { + if (WaitForSingleObject(write_mutex, INFINITE) != WAIT_OBJECT_0) { + return 1; + } + + return 0; +} + +int Serial::SerialImpl::writeUnlock() { + if (!ReleaseMutex(write_mutex)) { + return 1; + } + + return 0; +} + +}//serial +}//core +}//ydlidar +#endif // #if defined(_WIN32) + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.h new file mode 100644 index 0000000..74d9381 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/impl/windows/win_serial.h @@ -0,0 +1,175 @@ +#if defined(_WIN32) + +#ifndef SERIAL_IMPL_WINDOWS_H +#define SERIAL_IMPL_WINDOWS_H + +#include +#include +#include + +#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) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.cpp new file mode 100644 index 0000000..bab26bf --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.cpp @@ -0,0 +1,409 @@ +#include + +#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__) +# include +#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 &buffer, size_t size) { + ScopedReadLock lock(this->pimpl_); + uint8_t *buffer_ = static_cast(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(alloca(size * sizeof(uint8_t))); + size_t bytes_read = this->pimpl_->read(buffer_, size); + buffer.append(reinterpret_cast(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(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(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(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 Serial::readlines(size_t size, string eol) { + ScopedReadLock lock(this->pimpl_); + std::vector lines; + size_t eol_len = eol.length(); + uint8_t *buffer_ = static_cast(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(buffer_ + start_of_line), + read_so_far - start_of_line)); + } + + break; // Timeout occured on reading 1 byte + } + + if (string(reinterpret_cast + (buffer_ + read_so_far - eol_len), eol_len) == eol) { + // EOL found + lines.push_back(string(reinterpret_cast(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(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(data.c_str()), + data.length()); +} + +size_t Serial::write(const std::vector &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(); +} +} +} +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.h new file mode 100644 index 0000000..33ec226 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/core/serial/serial.h @@ -0,0 +1,710 @@ +#ifndef SERIAL_H +#define SERIAL_H + +#include +#include +#include +#include +#include +#include +#include + +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::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 &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 containing the lines. + * + */ + std::vector 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 &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 +list_ports(); + +} // namespace serial +}// namespace core +}// namespace ydlidar +#endif diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CMakeLists.txt new file mode 100644 index 0000000..b7aeb0c --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CMakeLists.txt @@ -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() diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.cpp new file mode 100644 index 0000000..41e4326 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.cpp @@ -0,0 +1,1881 @@ +// +// 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. +// +#include +#include +#include +#include +#include +#include "CYdLidar.h" +#include "core/math/angles.h" +#include "core/serial/common.h" +#include "core/common/DriverInterface.h" +#include "core/common/ydlidar_help.h" +#include "core/common/ydlidar_protocol.h" +#include "YDlidarDriver.h" +#include "ETLidarDriver.h" +#include "GSLidarDriver.h" +#include "SDMLidarDriver.h" +#include "DTSLidarDriver.h" +#include "TiaLidarDriver.h" + +using namespace std; +using namespace impl; +using namespace ydlidar::core; +using namespace ydlidar::core::common; +using namespace ydlidar::core::math; + +/*------------------------------------------------------------- + Constructor +-------------------------------------------------------------*/ +CYdLidar::CYdLidar() : lidarPtr(nullptr) +{ + m_SerialPort = "/dev/ydlidar"; + m_SerialBaudrate = 230400; + m_FixedResolution = false; + m_Reversion = false; + m_Inverted = false; // + m_AutoReconnect = true; + m_SingleChannel = false; + m_LidarType = TYPE_TRIANGLE; + m_MaxAngle = 180.f; + m_MinAngle = -180.f; + m_MaxRange = 64.0; + m_MinRange = 0.01f; + m_SampleRate = 5; + m_ScanFrequency = 10; + m_FixedSize = 720; + frequencyOffset = 0.4f; + m_AbnormalCheckCount = 2; + Major = 0; + Minjor = 0; + m_IgnoreArray.clear(); + m_IgnoreString = ""; + m_PointTime = static_cast(1e9 / 5000); + m_AngleOffset = 0.0f; + lidar_model = DriverInterface::YDLIDAR_G2B; + m_Intensity = false; + m_IntensityBit = 10; + last_node_time = getTime(); + global_nodes = new node_info[DriverInterface::MAX_SCAN_NODES]; + last_frequency = 0; + m_FristNodeTime = getTime(); + m_AllNode = 0; + m_DeviceType = YDLIDAR_TYPE_SERIAL; + m_SupportMotorDtrCtrl = true; + m_SupportHearBeat = false; + m_isAngleOffsetCorrected = false; + m_field_of_view = 360.f; + memset(&m_LidarVersion, 0, sizeof(LidarVersion)); + zero_offset_angle_scale = 4.f; +} + +/*------------------------------------------------------------- + ~CYdLidar +-------------------------------------------------------------*/ +CYdLidar::~CYdLidar() +{ + disconnecting(); + + if (global_nodes) + { + delete[] global_nodes; + global_nodes = NULL; + } +} + +bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) +{ + if (optval == NULL) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + + if (optname >= LidarPropFixedResolution) + { + if (optlen != sizeof(bool)) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + } + else if (optname >= LidarPropMaxRange) + { + if (optlen != sizeof(float)) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + } + else if (optname >= LidarPropSerialBaudrate) + { + if (optlen != sizeof(int)) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + } + else + { + } + + bool ret = true; + + switch (optname) + { + case LidarPropSerialPort: + m_SerialPort = (const char *)optval; + break; + + case LidarPropIgnoreArray: + m_IgnoreString = (const char *)optval; + m_IgnoreArray = ydlidar::split(m_IgnoreString, ','); + if (m_IgnoreArray.size() % 2 != 0) + { + m_IgnoreArray.clear(); + ret = false; + } + break; + + case LidarPropFixedResolution: + m_FixedResolution = *(bool *)(optval); + break; + + case LidarPropReversion: + m_Reversion = *(bool *)(optval); + break; + + case LidarPropInverted: + m_Inverted = *(bool *)(optval); + break; + + case LidarPropAutoReconnect: + m_AutoReconnect = *(bool *)(optval); + break; + + case LidarPropSingleChannel: + m_SingleChannel = *(bool *)(optval); + break; + + case LidarPropIntenstiy: + m_Intensity = *(bool *)(optval); + break; + case LidarPropIntenstiyBit: + m_IntensityBit = *(int *)(optval); + break; + + case LidarPropSupportMotorDtrCtrl: + m_SupportMotorDtrCtrl = *(bool *)(optval); + break; + + case LidarPropSupportHeartBeat: + m_SupportHearBeat = *(bool *)(optval); + break; + + case LidarPropMaxRange: + m_MaxRange = *(float *)(optval); + break; + + case LidarPropMinRange: + m_MinRange = *(float *)(optval); + break; + + case LidarPropMaxAngle: + m_MaxAngle = *(float *)(optval); + break; + + case LidarPropMinAngle: + m_MinAngle = *(float *)(optval); + break; + + case LidarPropScanFrequency: + m_ScanFrequency = *(float *)(optval); + break; + + case LidarPropSerialBaudrate: + m_SerialBaudrate = *(int *)(optval); + break; + + case LidarPropLidarType: + m_LidarType = *(int *)(optval); + break; + + case LidarPropDeviceType: + m_DeviceType = *(int *)(optval); + break; + + case LidarPropSampleRate: + { + int sr = *(int*)(optval); + m_SampleRate = sr; + break; + } + + case LidarPropAbnormalCheckCount: + m_AbnormalCheckCount = *(int *)(optval); + break; + + default: + ret = false; + break; + } + + return ret; +} + +bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) +{ + if (optval == NULL) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + + if (optname >= LidarPropFixedResolution) + { + if (optlen != sizeof(bool)) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + } + else if (optname >= LidarPropMaxRange) + { + if (optlen != sizeof(float)) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + } + else if (optname >= LidarPropSerialBaudrate) + { + if (optlen != sizeof(int)) + { +#if defined(_WIN32) + SetLastError(EINVAL); +#else + errno = EINVAL; +#endif + return false; + } + } + else + { + } + + bool ret = true; + + switch (optname) + { + case LidarPropSerialPort: + memcpy(optval, m_SerialPort.c_str(), optlen); + break; + + case LidarPropIgnoreArray: + memcpy(optval, m_IgnoreString.c_str(), optlen); + break; + + case LidarPropFixedResolution: + memcpy(optval, &m_FixedResolution, optlen); + break; + + case LidarPropReversion: + memcpy(optval, &m_Reversion, optlen); + break; + + case LidarPropInverted: + memcpy(optval, &m_Inverted, optlen); + break; + + case LidarPropAutoReconnect: + memcpy(optval, &m_AutoReconnect, optlen); + break; + + case LidarPropSingleChannel: + memcpy(optval, &m_SingleChannel, optlen); + break; + + case LidarPropIntenstiy: + memcpy(optval, &m_Intensity, optlen); + break; + case LidarPropIntenstiyBit: + memcpy(optval, &m_IntensityBit, optlen); + break; + + case LidarPropSupportMotorDtrCtrl: + memcpy(optval, &m_SupportMotorDtrCtrl, optlen); + break; + + case LidarPropSupportHeartBeat: + memcpy(optval, &m_SupportHearBeat, optlen); + break; + + case LidarPropMaxRange: + memcpy(optval, &m_MaxRange, optlen); + break; + + case LidarPropMinRange: + memcpy(optval, &m_MinRange, optlen); + break; + + case LidarPropMaxAngle: + memcpy(optval, &m_MaxAngle, optlen); + break; + + case LidarPropMinAngle: + memcpy(optval, &m_MinAngle, optlen); + break; + + case LidarPropScanFrequency: + memcpy(optval, &m_ScanFrequency, optlen); + break; + + case LidarPropSerialBaudrate: + memcpy(optval, &m_SerialBaudrate, optlen); + break; + + case LidarPropLidarType: + memcpy(optval, &m_LidarType, optlen); + break; + + case LidarPropDeviceType: + memcpy(optval, &m_DeviceType, optlen); + break; + + case LidarPropSampleRate: + { + int sr = m_SampleRate; + memcpy(optval, &sr, optlen); + break; + } + + case LidarPropAbnormalCheckCount: + memcpy(optval, &m_AbnormalCheckCount, optlen); + break; + + default: + ret = false; + break; + } + + return ret; +} + +/*------------------------------------------------------------- + initialize +-------------------------------------------------------------*/ +bool CYdLidar::initialize() +{ + uint32_t t = getms(); + if (!checkConnect()) + { + error("Error initializing YDLIDAR check Comms."); + return false; + } + + if (!checkStatus()) + { + error("Error initializing YDLIDAR check status under [%s] and [%d].", + m_SerialPort.c_str(), m_SerialBaudrate); + return false; + } + + info("Lidar init success, Elapsed time [%u]ms", getms() - t); + return true; +} + +/*------------------------------------------------------------- + initialize +-------------------------------------------------------------*/ +void CYdLidar::GetLidarVersion(LidarVersion &lv) +{ + memcpy(&lv, &m_LidarVersion, sizeof(LidarVersion)); + + std::string sn; + for (int i = 0; i < SDK_SNLEN; i++) + sn += char(lv.sn[i] + 48); + info("Lidar version\n" + "Firmware version: %u.%u.%u\n" + "Hardware version: %u\n" + "Serial: %s", + lv.soft_major, + lv.soft_minor, + lv.soft_patch, + lv.hardware, + sn.c_str()); +} + +/*------------------------------------------------------------- + turnOn +-------------------------------------------------------------*/ +bool CYdLidar::turnOn() +{ + if (lidarPtr->isscanning()) + return true; + + uint32_t t = getms(); + //启动扫描 + result_t ret = lidarPtr->startScan(); + if (!IS_OK(ret)) + { + ret = lidarPtr->startScan(); + if (!IS_OK(ret)) + { + lidarPtr->stop(); + error("Failed to start scan mode %d", ret); + return false; + } + } + info("Successed to start scan mode, Elapsed time %u ms", getms() - t); + + t = getms(); + //计算采样率 + if (!checkLidarAbnormal()) + { + lidarPtr->stop(); + error("Failed to turn on the Lidar, because the lidar is [%s].", + DriverInterface::DescribeDriverError(lidarPtr->getDriverError())); + return false; + } + info("Successed to check the lidar, Elapsed time %u ms", getms() - t); + + m_field_of_view = 360.f; + //网络TOF雷达需要设置视场角 + if (isNetTOFLidar(m_LidarType)) + { + lidarConfig cfg = lidarPtr->getFinishedScanCfg(); + m_field_of_view = cfg.fov_end - cfg.fov_start; + if (cfg.fov_end - 180 < m_MaxAngle) + { + m_MaxAngle = cfg.fov_end - 180; + } + if (cfg.fov_start - 180 > m_MinAngle) + { + m_MinAngle = cfg.fov_start - 180; + } + } + + last_frequency = 0; + m_FristNodeTime = getTime(); + m_AllNode = 0; + m_PointTime = lidarPtr->getPointTime(); + lidarPtr->setAutoReconnect(m_AutoReconnect); + info("Now lidar is scanning..."); + + lastStamp = 0; + //重置错误 + lidarPtr->setDriverError(NoError); + return true; +} + +bool CYdLidar::isScanning() const +{ + return lidarPtr && lidarPtr->isscanning(); +} + +bool CYdLidar::doProcessSimple(LaserScan &outscan) +{ + //判断是否已启动扫描 + if (!checkHardware()) + { + delay(200 / m_ScanFrequency); + m_AllNode = 0; + m_FristNodeTime = getTime(); + return false; + } + + size_t count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + + // wait Scan data: + uint64_t tim_scan_start = getTime(); + uint64_t startTs = tim_scan_start; + //从缓存中获取已采集的一圈扫描数据 + result_t op_result = lidarPtr->grabScanData(global_nodes, count, 1000); + uint64_t tim_scan_end = getTime(); + uint64_t endTs = tim_scan_end; + uint64_t sys_scan_time = tim_scan_end - tim_scan_start; //获取一圈数据所花费的时间 + outscan.points.clear(); + + // Fill in scan data: + if (IS_OK(op_result) && count) + { + int offsetSize = 0; + + if (isNetTOFLidar(m_LidarType)) + { + double echo_angle = static_cast(m_field_of_view * 1.0 / count); + + if (echo_angle != 0.0) + { + offsetSize = static_cast((360 - m_field_of_view) / echo_angle); + } + } + + //根据采样频率计算的采样间隔时间计算出来总的扫描时间 + uint64_t scan_time = m_PointTime * (count - 1 + offsetSize); + int timeDiff = static_cast(sys_scan_time - scan_time); + + bool HighPayLoad = false; + + if (global_nodes[0].stamp > 0 && + global_nodes[0].stamp < tim_scan_start) + { + tim_scan_end = global_nodes[0].stamp; + HighPayLoad = true; + } + + tim_scan_end -= m_PointTime; + tim_scan_end -= global_nodes[0].delayTime; + tim_scan_start = tim_scan_end - scan_time; + + if (!HighPayLoad && tim_scan_start < startTs) + { + tim_scan_start = startTs; + tim_scan_end = tim_scan_start + scan_time; + } + + if ((last_node_time + m_PointTime) >= tim_scan_start && + (last_node_time + m_PointTime) < endTs - scan_time) + { + tim_scan_start = last_node_time + m_PointTime; + tim_scan_end = tim_scan_start + scan_time; + } + + if (m_AllNode == 0 && abs(timeDiff) < 10 * 1e6) + { + m_FristNodeTime = tim_scan_start; + m_AllNode += (count + offsetSize); + } + else if (m_AllNode != 0) + { + m_AllNode += (count + offsetSize); + } + + last_node_time = tim_scan_end; + + if (m_MaxAngle < m_MinAngle) + { + float temp = m_MinAngle; + m_MinAngle = m_MaxAngle; + m_MaxAngle = temp; + } + + int all_node_count = count; + LaserDebug debug = {0}; + + memset(&debug, 0, sizeof(debug)); + outscan.config.min_angle = math::from_degrees(m_MinAngle); + outscan.config.max_angle = math::from_degrees(m_MaxAngle); + //将首末点采集时间差作为采集时长 + // outscan.config.scan_time = static_cast((global_nodes[count - 1].stamp - + // global_nodes[0].stamp)) / 1e9; + // outscan.config.scan_time = sys_scan_time / 1e9; + if (lastStamp > 0 && global_nodes[0].stamp > 0) + outscan.config.scan_time = double(global_nodes[0].stamp - lastStamp) / 1e9; + else + outscan.config.scan_time = 0; + lastStamp = global_nodes[0].stamp; + //计算时间增量 + if (!ISZERO(outscan.config.scan_time)) + outscan.config.time_increment = outscan.config.scan_time / (count - 1); + else + outscan.config.time_increment = .0f; + outscan.config.min_range = m_MinRange; + outscan.config.max_range = m_MaxRange; + //模组编号 + outscan.moduleNum = global_nodes[0].index; + //环境标记 + outscan.envFlag = global_nodes[0].is + (uint16_t(global_nodes[1].is) << 8); + //将一圈中第一个点采集时间作为该圈数据采集时间 + if (global_nodes[0].stamp > 0) + outscan.stamp = global_nodes[0].stamp; + else + outscan.stamp = 0; + + float scanfrequency = 0.0; + + if (m_FixedResolution) + { + all_node_count = m_FixedSize; + } + + outscan.config.angle_increment = math::from_degrees(m_field_of_view) / + (all_node_count - 1); + + float range = 0.0; + float intensity = 0.0; + float angle = 0.0; + debug.maxIndex = 0; + + // printf("AngleOffset %f\n", m_AngleOffset); + + //遍历一圈点 + for (int i = 0; i < count; i++) + { + const node_info& node = global_nodes[i]; + + // printf("%lu a:%.01f d:%u\n", + // i, float(node.angle) / 128.0f, node.dist); + + if (isNetTOFLidar(m_LidarType)) + { + angle = static_cast(global_nodes[i].angle / 100.0f) + + m_AngleOffset; + } + else + { + angle = static_cast((global_nodes[i].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f) + + m_AngleOffset; + } + + if (isOctaveLidar(lidar_model) || + isOldVersionTOFLidar(lidar_model, Major, Minjor)) + { + range = static_cast(global_nodes[i].dist / 2000.f); + } + else if (isR3Lidar(lidar_model)) + { + range = static_cast(global_nodes[i].dist / 40000.f); + } + else + { + if (isTOFLidar(m_LidarType) || + isNetTOFLidar(m_LidarType) || + isGSLidar(m_LidarType) || + isSDMLidar(m_LidarType) || + isDTSLidar(m_LidarType)) + { + range = static_cast(global_nodes[i].dist / 1000.f); + } + else + { + range = static_cast(global_nodes[i].dist / 4000.f); + } + } + + intensity = static_cast(global_nodes[i].qual); + + angle = math::from_degrees(angle); + + if (global_nodes[i].scanFreq != 0) + { + scanfrequency = global_nodes[i].scanFreq / 10.0; + + if (isTOFLidar(m_LidarType)) //TOF雷达转速偏移3Hz + { + if (!isOldVersionTOFLidar(lidar_model, Major, Minjor)) + { + scanfrequency = global_nodes[i].scanFreq / 10.0 + 3.0; + } + } + else if (isTEALidar(lidar_model) || + isGSLidar(m_LidarType) || + isTIALidar(m_LidarType)) //TEA雷达转速范围10~30，无缩放 + { + scanfrequency = global_nodes[i].scanFreq; + } + } + + // Rotate 180 degrees or not + if (m_Reversion || isNetTOFLidar(m_LidarType)) + { + angle = angle + M_PI; + } + + // Is it counter clockwise + if (m_Inverted) + { + angle = 2 * M_PI - angle; + } + + angle = math::normalize_angle(angle); + + // ignore angle + if (isRangeIgnore(angle)) + { + range = 0.0; + } + + //过滤点 + if (!isRangeValid(range) || + (m_SunNoise && node.is == SUNNOISEINTENSITY) || + (m_GlassNoise && node.is == GLASSNOISEINTENSITY)) + { + range = .0; + } + + // printf("i %d d %.03f a %.02f flag %u\n", + // i, range, angle*180.0/M_PI, node.sync); + + if (angle >= outscan.config.min_angle && + angle <= outscan.config.max_angle) + { + LaserPoint point; + point.angle = angle; + point.range = range; + point.intensity = intensity; + + outscan.points.push_back(point); + } + + parsePackageNode(global_nodes[i], debug); + if (global_nodes[i].error) + { + debug.maxIndex = 255; + } + } //end for (int i = 0; i < count; i++) + + outscan.size = outscan.points.size(); //保留原点云数 + + if (m_FixedResolution) + { + outscan.points.resize(all_node_count); + } + + //解析V2协议雷达扫描数据中ct信息中的设备信息 + // getDeviceInfoByPackage(debug); + //重新计算采样率 + resample(scanfrequency, count, tim_scan_end, tim_scan_start); + + outscan.scanFreq = scanfrequency; + outscan.sampleRate = m_SampleRate; + + return true; + } + else + { + // if (lidarPtr->getDriverError() != NoError) + { + fprintf(stderr, "[YDLIDAR ERROR]: %d %s\n", + op_result, + DriverInterface::DescribeDriverError(lidarPtr->getDriverError())); + fflush(stderr); + } + + m_AllNode = 0; + m_FristNodeTime = tim_scan_start; + } + + return false; +} + +/*------------------------------------------------------------- + turnOff +-------------------------------------------------------------*/ +bool CYdLidar::turnOff() +{ + if (lidarPtr) + { + if (lidarPtr->isscanning()) + info("Now lidar scanning has stopped!"); + lidarPtr->stop(); + } + + return true; +} + +/*------------------------------------------------------------- + disconnecting +-------------------------------------------------------------*/ +void CYdLidar::disconnecting() +{ + if (lidarPtr) + { + lidarPtr->disconnect(); + } +} + +/*------------------------------------------------------------- + getAngleOffset +-------------------------------------------------------------*/ +float CYdLidar::getAngleOffset() const +{ + return m_AngleOffset; +} + +/*------------------------------------------------------------- + isAngleOffsetCorrected +-------------------------------------------------------------*/ +bool CYdLidar::isAngleOffsetCorrected() const +{ + return m_isAngleOffsetCorrected; +} + +/*------------------------------------------------------------- + DescribeError +-------------------------------------------------------------*/ +const char *CYdLidar::DescribeError() const +{ + char const *value = ""; + + if (lidarPtr) + { + return lidarPtr->DescribeError(); + } + + return value; +} + +/*------------------------------------------------------------- + getDriverError +-------------------------------------------------------------*/ +DriverError CYdLidar::getDriverError() const +{ + DriverError er = UnknownError; + + if (lidarPtr) + { + return lidarPtr->getDriverError(); + } + + return er; +} + +bool CYdLidar::setWorkMode(int mode, uint8_t addr) +{ + if (lidarPtr) + return (lidarPtr->setWorkMode(mode, addr) == RESULT_OK); + else + return false; +} + +void CYdLidar::enableSunNoise(bool e) +{ + m_SunNoise = e; +} + +void CYdLidar::enableGlassNoise(bool e) +{ + m_GlassNoise = e; +} + +bool CYdLidar::getUserVersion(std::string &version) +{ + if (!checkHardware()) + { + error("Device is not open!"); + return false; + } + + size_t count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + result_t op_result = lidarPtr->grabScanData(global_nodes, count); + if (IS_OK(op_result) && count > 2) + { + uint8_t userVerion = global_nodes[USERVERSIONNDEX].debugInfo; + version = std::to_string(userVerion & 0xc0) + "." + std::to_string(userVerion & 0x3f); + return true; + } + + return false; +} + +void CYdLidar::setBottomPriority(bool yes) +{ + m_Bottom = yes; +} + +bool CYdLidar::getDeviceInfo(device_info& di, int type) +{ + if (lidarPtr) + return lidarPtr->getDeviceInfoEx(di, type); + + return false; +} + +bool CYdLidar::getDeviceInfo(std::vector& dis) +{ + if (lidarPtr) + return IS_OK(lidarPtr->getDeviceInfo(dis)); + return false; +} + +void CYdLidar::setAutoIntensity(bool yes) +{ + m_AutoIntensity = yes; +} + +bool CYdLidar::getPitchAngle(float& pitch) +{ + if (lidarPtr) + return lidarPtr->getPitchAngle(pitch); + return false; +} + +bool CYdLidar::ota() +{ + if (lidarPtr) + return lidarPtr->ota(); + return false; +} + +/*------------------------------------------------------------- + isRangeValid +-------------------------------------------------------------*/ +bool CYdLidar::isRangeValid(double reading) const +{ + if (reading >= m_MinRange && reading <= m_MaxRange) + { + return true; + } + + return false; +} + +/*------------------------------------------------------------- + isRangeIgnore +-------------------------------------------------------------*/ +bool CYdLidar::isRangeIgnore(double angle) const +{ + bool ret = false; + + for (uint16_t j = 0; j < m_IgnoreArray.size(); j = j + 2) + { + if ((math::from_degrees(m_IgnoreArray[j]) <= angle) && + (angle <= math::from_degrees(m_IgnoreArray[j + 1]))) + { + ret = true; + break; + } + } + + return ret; +} + +bool CYdLidar::getDeviceInfoByPackage(const LaserDebug &debug) +{ + if (!lidarPtr) + return false; + + device_info di; + memset(&di, 0, DEVICEINFOSIZE); + + if (parseLaserDebugInfo(debug, di)) + { + if (printfDeviceInfo(di, EPT_Module)) + { + std::string serial_number; + Major = (uint8_t)(di.firmware_version >> 8); + Minjor = (uint8_t)(di.firmware_version & 0xff); + m_LidarVersion.hardware = di.hardware_version; + m_LidarVersion.soft_major = Major; + m_LidarVersion.soft_minor = Minjor / 10; + m_LidarVersion.soft_patch = Minjor % 10; + memcpy(&m_LidarVersion.sn[0], &di.serialnum[0], 16); + + for (int i = 0; i < 16; i++) + { + serial_number += std::to_string(di.serialnum[i] & 0xff); + } + + m_SerialNumber = serial_number; + //设置模组标记 + lidarPtr->setHasDeviceInfo(lidarPtr->getHasDeviceInfo() | EPT_Module); + + return true; + } + } + + return false; +} + +/*------------------------------------------------------------- + resample +-------------------------------------------------------------*/ +void CYdLidar::resample( + int frequency, int count, + uint64_t tim_scan_end, + uint64_t tim_scan_start) +{ + //重新校准采样率 + // if( (lidar_model == DriverInterface::YDLIDAR_TG15) + // || (lidar_model == DriverInterface::YDLIDAR_TG30) + // || (lidar_model == DriverInterface::YDLIDAR_TG50) ) + // { + // m_SampleRate = m_SampleRatebyD1; + // } + + if (frequency > 3 && frequency <= 15.7 && + (frequency - last_frequency) < 0.05) + { + int sample = static_cast((frequency * count + 500) / 1000); + + if (sample != m_SampleRate) + { + } + } + + last_frequency = frequency; + int realSampleRate = 0; + + if (m_AllNode != 0) + { + realSampleRate = 1e9 * m_AllNode / (tim_scan_end - m_FristNodeTime); + int RateDiff = std::abs(static_cast(realSampleRate - m_SampleRate * 1000)); + + if (RateDiff > 1000 || + (static_cast(tim_scan_end - m_FristNodeTime) > 10 * 1e9 && + RateDiff > 30)) + { + m_AllNode = 0; + m_FristNodeTime = tim_scan_start; + } + } +} + +//检查异常 +bool CYdLidar::checkLidarAbnormal() +{ + size_t count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + int checkCount = 0; //检查次数 + + if (m_AbnormalCheckCount < 2) + m_AbnormalCheckCount = 2; + + result_t ret = RESULT_FAIL; + std::vector data; + + while (checkCount < m_AbnormalCheckCount) + { + // printf("checkLidarAbnormal %d\n", checkCount); + + // Ensure that the voltage is insufficient or the motor resistance is high, + //causing an abnormality. + if (checkCount) + delay(500); + + float scan_time = 0.0; + uint64_t start_time = 0; + uint64_t end_time = 0; + int checkOneCount = 0; + ret = RESULT_OK; + + //单双通雷达，计算采样率 + while (checkOneCount < 5 && + // (scan_time < 0.05 || !lidarPtr->getSingleChannel()) && + IS_OK(ret)) + { + checkOneCount ++; + start_time = getTime(); + count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + ret = lidarPtr->grabScanData(global_nodes, count); + end_time = getTime(); + scan_time = 1.0 * static_cast(end_time - start_time) / 1e9; + + if (IS_OK(ret)) + { + // 获取CT信息 + if (!(lidarPtr->getHasDeviceInfo() & EPT_Module)) + { + // printf("Get module device info\n"); + LaserDebug debug = {0}; + for (int i = 0; i < count; ++i) + { + parsePackageNode(global_nodes[i], debug); + if (global_nodes[i].error) + debug.maxIndex = 255; + } + // 解析V2协议雷达扫描数据中ct信息中的设备信息 + getDeviceInfoByPackage(debug); + } + + if (isNetTOFLidar(m_LidarType)) + { + return IS_OK(ret); + } + + data.push_back(count); + if (std::abs(static_cast(data.front() - count)) > 10) + data.erase(data.begin()); + + if (calcSampleRate(count, scan_time)) + { + // 双通雷达计算完采样率即可返回 + if (!lidarPtr->getSingleChannel()) + { + return IS_OK(ret); + } + } + else + { + //计算采样率 + if (scan_time > 0.05 && scan_time < 0.5) + { + m_SampleRate = static_cast((count / scan_time + 500) / 1000); + m_PointTime = 1e9 / (m_SampleRate * 1000); + lidarPtr->setPointTime(m_PointTime); + } + } + } + } + + //单通雷达计算固定分辨率时的一圈点数 + if (lidarPtr->getSingleChannel() && + data.size() > 1) + { + int total = accumulate(data.begin(), data.end(), 0); + int mean = total / data.size(); // mean value + m_FixedSize = (static_cast((mean + 5) / 10)) * 10; + info("Single Fixed Size: %d", m_FixedSize); + info("Sample Rate: %.02fK", m_SampleRate); + return true; + } + + checkCount ++; + } + + return IS_OK(ret); +} + +/*------------------------------------------------------------- + removeExceptionSample +-------------------------------------------------------------*/ +inline void removeExceptionSample(std::map &smap) +{ + if (smap.size() < 2) + { + return; + } + + std::map::iterator last = smap.begin(); + std::map::iterator its = smap.begin(); + + while (its != smap.end()) + { + if (last->second > its->second) + { + smap.erase(its++); + } + else if (last->second < its->second) + { + its = smap.erase(last); + last = its; + its++; + } + else + { + its++; + } + } +} + +/*------------------------------------------------------------- + calcSampleRate +-------------------------------------------------------------*/ +bool CYdLidar::calcSampleRate(int count, double scan_time) +{ + if (count < 1) + return false; + + // 1、如果雷达支持直接获取转速，则使用获取的转速计算采样率，此时将默认采样率值置为该值 + // 2、如果有设置默认采样率，判断当前雷达型号对应的默认采样率值的个数，个数为1， + //直接使用该采样率，个数不为1则根据实时采样率进行匹配 + // 3、如果没有设置默认采样率，则使用实时采样率 + float sr = 0; + bool ret = false; + + if (global_nodes[0].scanFreq != 0) + { + //如果解析到转速信息，根据转速计算采样率 + double scanfrequency = global_nodes[0].scanFreq / 10.0; + if (isTOFLidar(m_LidarType) && + !isOldVersionTOFLidar(lidar_model, Major, Minjor)) + { + scanfrequency = global_nodes[0].scanFreq / 10.0 + 3.0; + } + sr = static_cast((count * scanfrequency + 500) / 1000); + + if (isSDMLidar(m_LidarType)) + { + defalutSampleRate.clear(); //SDM雷达通过协议获取转速计算采样率 + sr = float(count * global_nodes[0].scanFreq) / 1000; + } + } + else + { + //如果没有解析到转速信息，根据时间计算采样率 + if (scan_time > 0.04 && scan_time < 0.4) + { + sr = static_cast((count / scan_time + 500) / 1000); + } + } + + size_t size = defalutSampleRate.size(); + if (size) + { + if (size == 1) + { + sr = defalutSampleRate.front(); + ret = true; + } + else + { + float d = .0; + for (size_t i=0; i 0) + SampleRateMap[sr * 1000] ++; //放大1000倍存入 + if (isValidSampleRate(SampleRateMap)) + ret = true; + } + + if (ret) + { + m_SampleRate = sr; + m_PointTime = 1e9 / (m_SampleRate * 1000); + lidarPtr->setPointTime(m_PointTime); + if (!m_SingleChannel) + // m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); //不知转速为何要减少0.1 + m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency); + + info("Scan Frequency: %.02fHz", m_ScanFrequency); + if (!isSDMLidar(m_LidarType)) //非SDM雷达才打印Fixed Size + info("Fixed Size: %d", m_FixedSize); + info("Sample Rate: %.02fK", m_SampleRate); + } + + return ret; +} + +/*------------------------------------------------------------- + getDeviceHealth +-------------------------------------------------------------*/ +bool CYdLidar::getDeviceHealth() +{ + if (!lidarPtr) + { + return false; + } + + result_t ret; + device_health healthinfo; + memset(&healthinfo, 0, sizeof(device_health)); + ret = lidarPtr->getHealth(healthinfo, + DriverInterface::DEFAULT_TIMEOUT / 2); + + if (IS_OK(ret)) + { + info("Lidar running correctly! The health status %s", + healthinfo.status == 0 ? "good" : "bad"); + if (healthinfo.status == 2) + { + error("Error, Lidar internal error[0x%X] detected. " + "Please reboot the device to retry.", + healthinfo.error_code); + return false; + } + else + { + return true; + } + } + else + { + error("Error, cannot retrieve Lidar health code %d", ret); + return false; + } +} + +/*------------------------------------------------------------- + getDeviceInfo +-------------------------------------------------------------*/ +bool CYdLidar::getDeviceInfo() +{ + if (!lidarPtr) + return false; + + bool ret = false; + device_info di; + memset(&di, 0, sizeof(device_info)); + result_t op_result = lidarPtr->getDeviceInfo(di, + DriverInterface::DEFAULT_TIMEOUT / 2); + if (!IS_OK(op_result)) + { + // error("Fail to get baseplate device information!"); + // return false; + } + + if (!isSupportLidar(di.model)) + { + error("Current SDK does not support current lidar model [%s]", + lidarModelToString(di.model).c_str()); + return false; + } + + // check Lidar Type Config + if (isTOFLidarByModel(di.model)) + { + if (!isTOFLidar(m_LidarType)) + { + error("Incorrect Lidar Type setting..."); + m_LidarType = TYPE_TOF; + lidarPtr->setLidarType(m_LidarType); + } + } + else + { + // if (!isTriangleLidar(m_LidarType) && + // !isNetTOFLidarByModel(devinfo.model) && + // !m_SingleChannel) + // { + // error("Incorrect Lidar Type setting, Reset Type to %d...\n", + // TYPE_TRIANGLE); + // m_LidarType = TYPE_TRIANGLE; + // lidarPtr->setLidarType(m_LidarType); + // } + } + + frequencyOffset = 0.4; + lidar_model = di.model; + info("Current Lidar Model Code %d", lidar_model); + // bool intensity = hasIntensity(di.model); + // intensity = m_Intensity; + // lidarPtr->setIntensities(intensity); + // printf("Set Lidar Intensity Bit count %d\n", m_IntensityBit); + // lidarPtr->setIntensityBit(m_IntensityBit); + defalutSampleRate = getDefaultSampleRate(di.model); + // printf("getDefaultSampleRate %d\n", defalutSampleRate.size()); + + std::string serial_number; + ret = true; + + if (printfDeviceInfo(di, EPT_Base)) + { + Major = (uint8_t)(di.firmware_version >> 8); + Minjor = (uint8_t)(di.firmware_version & 0xff); + m_LidarVersion.hardware = di.hardware_version; + m_LidarVersion.soft_major = Major; + if (isGSLidar(m_LidarType)) + { + m_LidarVersion.soft_minor = Minjor; + m_LidarVersion.soft_patch = 0; + } + else + { + m_LidarVersion.soft_minor = Minjor / 10; + m_LidarVersion.soft_patch = Minjor % 10; + } + memcpy(&m_LidarVersion.sn[0], &di.serialnum[0], SDK_SNLEN); + + for (int i = 0; i < SDK_SNLEN; i++) + { + serial_number += std::to_string(di.serialnum[i] & 0xff); + } + + m_SerialNumber = serial_number; + zero_offset_angle_scale = lidarZeroOffsetAngleScale( + di.model, di.firmware_version >> 8, di.firmware_version & 0x00ff); + } + + // uint32_t t = getms(); + if (hasSampleRate(di.model)) + { + checkSampleRate(); + } + else + { + if (defalutSampleRate.size()) + { + m_PointTime = 1e9 / (defalutSampleRate.front() * 1000); + lidarPtr->setPointTime(m_PointTime); + } + } + + // printf("LIDAR get device info finished, Elapsed time %u ms\n", getms() - t); + //检查转速 + if (hasScanFrequencyCtrl(di.model) || + ((isTOFLidar(m_LidarType)) && !m_SingleChannel) || + isNetTOFLidar(m_LidarType)) + { + checkScanFrequency(); + } + + if (isSupportHeartBeat(di.model)) + { + ret &= checkHeartBeat(); + + if (!ret) + { + fprintf(stderr, "Failed to Set HeartBeat[%d].\n", m_SupportHearBeat); + } + } + + if (hasZeroAngle(di.model)) + { + ret &= checkCalibrationAngle(serial_number); + } + + return ret; +} + +void CYdLidar::handleSingleChannelDevice() +{ + if (!lidarPtr || + lidarPtr->getBottom() || + !lidarPtr->getSingleChannel()) + { + return; + } + + //获取模组设备信息 + //1、单通雷达需要从CT信息中获取模组设备信息 + //2、双通雷达需要获取启动时抛出的模组设备信息 + + device_info di; + memset(&di, 0, sizeof(device_info)); + + result_t op_result = lidarPtr->getDeviceInfo(di); + if (!IS_OK(op_result)) + { + return; + } + + if (printfDeviceInfo(di, EPT_Module)) + { + m_LidarVersion.hardware = di.hardware_version; + m_LidarVersion.soft_major = Major; + m_LidarVersion.soft_minor = Minjor / 10; + m_LidarVersion.soft_patch = Minjor % 10; + memcpy(&m_LidarVersion.sn[0], &di.serialnum[0], SDK_SNLEN); + } + + lidar_model = di.model; + // defalutSampleRate = getDefaultSampleRate(devinfo.model); + + info("Single channel current sampling rate: %.02fK", m_SampleRate); + return; +} + +/*------------------------------------------------------------- + checkSampleRate +-------------------------------------------------------------*/ +void CYdLidar::checkSampleRate() +{ + sampling_rate _rate = {0}; + int sr = 0; + int try_count = 0; + m_FixedSize = 1440; + result_t ret = lidarPtr->getSamplingRate(_rate); + if (IS_OK(ret)) + { + info("Origin sample rate code: %u", _rate.rate); + if (!isTOFLidarByModel(lidar_model)) + { + //非TG系列雷达获取采样率码转成采样率值 + sr = ConvertUserToLidarSmaple(lidar_model, m_SampleRate, _rate.rate); + //非TG系列雷达通过设备信息获取 + while (sr != _rate.rate) + { + ret = lidarPtr->setSamplingRate(_rate); + try_count++; + if (try_count > 3) + { + break; + } + } + sr = ConvertLidarToUserSmaple(lidar_model, _rate.rate); + } + else + { + //TG系列雷达直接获取采样率值 + sr = ConvertLidarToUserSmaple(lidar_model, _rate.rate); + } + + m_SampleRate = sr; + defalutSampleRate.clear(); + defalutSampleRate.push_back(m_SampleRate); + info("Current sample rate: %.02fK", m_SampleRate); + } +} + +bool CYdLidar::checkScanFrequency() +{ + float frequency = 7.4f; + scan_frequency _scan_frequency; + float hz = 0.f; + result_t ans = RESULT_FAIL; + + if (isSupportScanFrequency(lidar_model, m_ScanFrequency)) + { + //TODO: 此处为何要加上偏移量，待解释 + // m_ScanFrequency += frequencyOffset; + ans = lidarPtr->getScanFrequency(_scan_frequency); + if (IS_OK(ans)) + { + frequency = _scan_frequency.frequency / 100.f; + if (isTOFLidar(m_LidarType)) //TG雷达转速虚高0.4需要减去还原真实转速 + frequency -= 0.4; + hz = m_ScanFrequency - frequency; + info("Current scan frequency: %.02fHz", frequency); + if (hz > 0) + { + //大调速 + while (hz > 0.95) + { + lidarPtr->setScanFrequencyAdd(_scan_frequency); + hz -= 1.0; + } + //小调速 + while (hz > 0.09) + { + lidarPtr->setScanFrequencyAddMic(_scan_frequency); + hz -= 0.1; + } + + frequency = _scan_frequency.frequency / 100.0f; + } + else + { + while (hz < -0.95) + { + lidarPtr->setScanFrequencyDis(_scan_frequency); + hz = hz + 1.0; + } + + while (hz < -0.09) + { + lidarPtr->setScanFrequencyDisMic(_scan_frequency); + hz = hz + 0.1; + } + + frequency = _scan_frequency.frequency / 100.0f; + } + } + } + else + { + // m_ScanFrequency += frequencyOffset; + error("Current scan frequency[%f] is out of range.", + m_ScanFrequency); + } + + ans = lidarPtr->getScanFrequency(_scan_frequency); + if (IS_OK(ans)) + { + frequency = _scan_frequency.frequency / 100.0f; + if (isTOFLidar(m_LidarType)) //TG雷达转速虚高0.4需要减去还原真实转速 + frequency -= 0.4; + m_ScanFrequency = frequency; + } + + // if( (lidar_model == DriverInterface::YDLIDAR_TG15) + // || (lidar_model == DriverInterface::YDLIDAR_TG30) + // || (lidar_model == DriverInterface::YDLIDAR_TG50) ) + // { + // m_SampleRate = m_SampleRatebyD1; + // } + + // m_ScanFrequency -= frequencyOffset; + m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); + info("Current scan frequency: %.02fHz", m_ScanFrequency); + // info("Fixed size: %d", m_FixedSize); + return true; +} + +bool CYdLidar::checkHeartBeat() +{ + if (!m_SupportHearBeat) + { + lidarPtr->setHeartBeat(false); + return true; + } + + bool ret = false; + scan_heart_beat beat; + int retry = 0; + + do + { + result_t ans = lidarPtr->setScanHeartbeat(beat); + + if (IS_OK(ans) && !beat.enable) + { + ret = true; + break; + } + + retry++; + } while (retry < 4); + + lidarPtr->setHeartBeat(m_SupportHearBeat); + return ret; +} + +/*------------------------------------------------------------- + checkCalibrationAngle +-------------------------------------------------------------*/ +bool CYdLidar::checkCalibrationAngle(const std::string &serialNumber) +{ + bool ret = false; + m_AngleOffset = 0.0; + result_t ans = RESULT_FAIL; + offset_angle angle; + int retry = 0; + m_isAngleOffsetCorrected = false; + + while (retry < 2) + { + ans = lidarPtr->getZeroOffsetAngle(angle); + + if (IS_OK(ans)) + { + if (angle.angle > zero_offset_angle_scale * 360 || + angle.angle < -zero_offset_angle_scale * 360) + { + ans = lidarPtr->getZeroOffsetAngle(angle); + + if (!IS_OK(ans)) + { + retry++; + continue; + } + } + + m_isAngleOffsetCorrected = (angle.angle != 180 * zero_offset_angle_scale); + m_AngleOffset = angle.angle / zero_offset_angle_scale; + ret = true; + info("Successfully obtained the %s offset angle[%f] from the lidar[%s]", + m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset, + serialNumber.c_str()); + return ret; + } + + retry++; + } + + info("Current %s AngleOffset : %f°", + m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset); + return ret; +} + +/*------------------------------------------------------------- + checkConnect +-------------------------------------------------------------*/ +bool CYdLidar::checkConnect() +{ + //如果雷达类型有变化则需要先删除旧对象 + if (lidarPtr && + lidarPtr->getLidarType() != m_LidarType) + { + delete lidarPtr; + lidarPtr = nullptr; + } + //如果未创建对象 + if (!lidarPtr) + { + info("SDK initializing"); + + //根据雷达类型创建对应的实例 + if (isNetTOFLidar(m_LidarType)) + lidarPtr = new ydlidar::ETLidarDriver(); //T15 + else if (isGSLidar(m_LidarType)) //GS + lidarPtr = new ydlidar::GSLidarDriver(m_DeviceType); + else if (isSDMLidar(m_LidarType)) //SDM + lidarPtr = new ydlidar::SDMLidarDriver(); + else if (isDTSLidar(m_LidarType)) //SDM + lidarPtr = new ydlidar::DTSLidarDriver(); + else if (isTIALidar(m_LidarType)) + lidarPtr = new ydlidar::TiaLidarDriver(); + else //通用雷达 + lidarPtr = new ydlidar::YDlidarDriver(m_DeviceType); + + if (!lidarPtr) + { + error("Create driver fail!"); + return false; + } + + info("SDK has been initialized"); + info("SDK Version: %s", lidarPtr->getSDKVersion().c_str()); + } + + if (lidarPtr->isconnected()) + { + return true; + } + + //初始化 + lidarPtr->setSingleChannel(m_SingleChannel); + lidarPtr->setLidarType(m_LidarType); + lidarPtr->setScanFreq(m_ScanFrequency); + lidarPtr->setSampleRate(m_SampleRate); //设置采样率 + lidarPtr->setSupportMotorDtrCtrl(m_SupportMotorDtrCtrl); + lidarPtr->setBottom(m_Bottom); + lidarPtr->setDebug(m_Debug); + lidarPtr->setOtaName(otaName); + lidarPtr->setOtaEncode(otaEncode); + lidarPtr->setIntensities(m_Intensity); + lidarPtr->setIntensityBit(m_IntensityBit); + lidarPtr->setAutoIntensity(m_AutoIntensity); + + uint32_t t = getms(); + + // Is it COMX, X>4? -> "\\.\COMX" + if (m_SerialPort.size() >= 3) + { + if (tolower(m_SerialPort[0]) == 'c' && + tolower(m_SerialPort[1]) == 'o' && + tolower(m_SerialPort[2]) == 'm') + { + // Need to add "\\.\"? + if (m_SerialPort.size() > 4 || m_SerialPort[3] > '4') + { + m_SerialPort = std::string("\\\\.\\") + m_SerialPort; + } + } + } + //连接 + result_t op_result = lidarPtr->connect(m_SerialPort.c_str(), m_SerialBaudrate); + if (!IS_OK(op_result)) + { + if (isNetTOFLidar(m_LidarType)) + { + error("Error, cannot bind to the specified IP Address[%s]", + m_SerialPort.c_str()); + } + else + { + error("Error, cannot bind to the specified [%s:%s] and [%s:%d]", + m_DeviceType != YDLIDAR_TYPE_SERIAL ? "IP Address" : "serial port", + m_SerialPort.c_str(), + m_DeviceType != YDLIDAR_TYPE_SERIAL ? "network port" : "baudrate", + m_SerialBaudrate); + } + + return false; + } + + info("Connect elapsed time %u ms", getms() - t); + info("Lidar successfully connected [%s:%d]", + m_SerialPort.c_str(), m_SerialBaudrate); + return true; +} + +/*------------------------------------------------------------- + checkStatus +-------------------------------------------------------------*/ +bool CYdLidar::checkStatus() +{ + uint32_t t = getms(); + getDeviceHealth(); + getDeviceInfo(); + info("Check status, Elapsed time %u ms", getms() - t); + + return true; +} + +/*------------------------------------------------------------- + checkHardware +-------------------------------------------------------------*/ +bool CYdLidar::checkHardware() +{ + if (!lidarPtr) + return false; + + return lidarPtr->isscanning(); +} + +namespace ydlidar +{ + void os_init() + { + ydlidar::core::base::init(); + } + + bool os_isOk() + { + return ydlidar::core::base::ok(); + } + + void os_shutdown() + { + ydlidar::core::base::shutdown(); + } + + std::map lidarPortList() + { + return ydlidar::YDlidarDriver::lidarPortList(); + } + +//打印logo字符 +void printLogo() +{ + info("__ ______ _ ___ ____ _ ____"); + info("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\"); + info(" \\ V /| | | | | | || | | |/ _ \\ | |_) |"); + info(" | | | |_| | |___ | || |_| / ___ \\| _ <"); + info(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\"); + info(""); +} + +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.h new file mode 100644 index 0000000..67a75cb --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/CYdLidar.h @@ -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) + +
Library CYdLidar +
File CYdLidar.h +
Author Tony [code at ydlidar com] +
Source https://github.com/ydlidar/YDLidar-SDK +
Version 1.0.0 +
Sample [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] +
+ 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 +#include +#include +#include +#include + + +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& 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 SampleRateMap; ///< Sample Rate Map + std::string m_SerialNumber; ///< LiDAR serial number + // int defalutSampleRate; ///< LiDAR Default Sampling Rate + std::vector 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 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 lidarPortList(); +//打印logo字符 +YDLIDAR_API void printLogo(); + +} + + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.cpp new file mode 100644 index 0000000..64d7cc7 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.cpp @@ -0,0 +1,995 @@ +#include +#include +#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(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 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 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(cs >> 8); // 将高字节赋给下标为 0 的元素 +// buff[1] = static_cast(cs); // 将低字节赋给下标为 1 的元素 + buff[size] = static_cast(cs >> 8); //将高字节赋给下标为 0 的元素; + buff[size+1] = static_cast(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 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 &data, + uint32_t timeout) +{ + int pos = 0; + uint32_t st = getms(); + uint32_t wt = 0; + vector recvBuff(SDK_DTS_BUFFLEN, 0); + uint16_t cs = 0; + uint8_t dataSize = 0; + vector 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 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 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 &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; +} + +} + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.h new file mode 100644 index 0000000..07b8667 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/DTSLidarDriver.h @@ -0,0 +1,208 @@ +#ifndef DTSLIDARDRIVER_H +#define DTSLIDARDRIVER_H +#include +#include +#include +#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 &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& data); + +private: + serial::Serial *_serial = nullptr; //串口 + std::vector recvBuff; //一包数据缓存 + float k = 0; //校准参数k + float b = 0; //校准参数b +}; +} +#endif // DTSLIDARDRIVER_H diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.cpp new file mode 100644 index 0000000..429e076 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.cpp @@ -0,0 +1,1278 @@ +/***************************************************************************** +* EAI TOF LIDAR DRIVER * +* Copyright (C) 2018-2020 EAI TEAM chushuifurong618@eaibot.com. * +* * +* This file is part of EAI TOF LIDAR DRIVER. * +* * +* @file ETLidarDriver.cpp * +* @brief TOF LIDAR DRIVER * +* Details. * +* * +* @author Tony.Yang * +* @email chushuifurong618@eaibot.com * +* @version 1.0.0(版本号) * +* @date chushuifurong618@eaibot.com * +* * +* * +*----------------------------------------------------------------------------* +* Remark : Description * +*----------------------------------------------------------------------------* +* Change History : * +* | | | * +*----------------------------------------------------------------------------* +* 2018/08/09 | 1.0.0 | Tony.Yang | Create file * +*----------------------------------------------------------------------------* +* * +*****************************************************************************/ +#if defined(_MSC_VER) +#define NOMINMAX +#endif + +#include +#include "ETLidarDriver.h" +#include +/*Socket Specific headers */ +#include +#include +#include +#include +#include +#include +using namespace impl; + +using namespace ydlidar; +using namespace ydlidar::core; +using namespace ydlidar::core::network; +using namespace ydlidar::core::base; + + +///////////////////////////////////////////////////////////////////////////////////////// +// port defaults to 9000 if not provided. +ETLidarDriver::ETLidarDriver() : + offset_len(0), + port(9000), + m_sampleRate(20000), + m_force_update(false) { + + socket_cmd = new CActiveSocket(CSimpleSocket::SocketTypeTcp); + socket_data = new CPassiveSocket(CSimpleSocket::SocketTypeUdp); + socket_data->SetSocketType(CSimpleSocket::SocketTypeUdp); + socket_cmd->SetConnectTimeout(DEFAULT_CONNECTION_TIMEOUT_SEC, + DEFAULT_CONNECTION_TIMEOUT_USEC); + scan_node_buf = new node_info[MAX_SCAN_NODES]; + scan_node_count = 0; + m_lastAngle = 0.f; + m_currentAngle = 0.f; + nodeIndex = 0; + retryCount = 0; + isAutoReconnect = true; + isAutoconnting = false;; + m_SupportMotorDtrCtrl = true; + m_AbnormalCheckCount = 4; + + m_isScanning = false; + m_isConnected = false; + m_port = "192.168.0.11"; + m_baudrate = 8000; + m_config.motor_rpm = 1200; + m_config.laserScanFrequency = 50; + m_config.correction_angle = 20640; + m_config.correction_distance = 6144; + m_SingleChannel = false; + m_LidarType = TYPE_TOF_NET; + m_PointTime = 1e9 / 20000; + m_isValidDevice = true; + +} + +///////////////////////////////////////////////////////////////////////////////////////// +ETLidarDriver::~ETLidarDriver() { + disconnect(); + isAutoReconnect = false; + ScopedLocker data_lock(_data_lock); + + if (socket_data) { + delete socket_data; + socket_data = NULL; + } + + ScopedLocker lock(_cmd_lock); + + if (socket_cmd) { + delete socket_cmd; + socket_cmd = NULL; + } + + if (scan_node_buf) { + delete[] scan_node_buf; + scan_node_buf = nullptr; + } +} + +void ETLidarDriver::updateScanCfg(const lidarConfig &config) { + if (m_isConnected) { + return; + } + + m_force_update = true; + m_user_config = config; + +} + +result_t ETLidarDriver::connect(const char *port_path, uint32_t baudrate) { + m_port = port_path; + m_baudrate = baudrate; + m_isConnected = false; + + if (!configPortConnect(m_port.c_str(), port)) { + setDriverError(NotOpenError); + m_isValidDevice = false; + return RESULT_FAIL; + } + + m_isValidDevice = true; + disConfigConnect(); + lidarConfig config; + bool ret = getScanCfg(config); + + if (!ret) { + return RESULT_FAIL; + } + + if (config.dataRecvPort != baudrate) { + m_baudrate = config.dataRecvPort; +// if (!m_force_update) { +// m_user_config = config; +// m_user_config.dataRecvPort = baudrate; +// m_force_update = true; +// } + } + + if (m_force_update) { + setScanCfg(m_user_config); + } else { + m_user_config = config; + } + + if (!dataPortConnect(m_config.deviceIp, m_config.dataRecvPort)) { + stopMeasure(); + setDriverError(NotOpenError); + return RESULT_FAIL; + } + + m_isConnected = true; + return RESULT_OK; +} + +bool ETLidarDriver::isconnected() const { + return m_isConnected; +} + +bool ETLidarDriver::isscanning() const { + return m_isScanning; +} + +void ETLidarDriver::setIntensities(const bool &isintensities) { + m_intensities = isintensities; +} + +void ETLidarDriver::setAutoReconnect(const bool &enable) { + isAutoReconnect = enable; +} + +const char *ETLidarDriver::DescribeError(bool isTCP) { + if (isTCP) { + ScopedLocker lock(_cmd_lock); + return socket_cmd != NULL ? socket_cmd->DescribeError() : "NO Socket"; + } else { + ScopedLocker lock(_data_lock); + return socket_data != NULL ? socket_data->DescribeError() : "NO Socket"; + } +} + +bool ETLidarDriver::configPortConnect(const char *lidarIP, int tcpPort) { + + ScopedLocker lock(_cmd_lock); + + if (!socket_cmd) { + return false; + } + + if (!socket_cmd->IsSocketValid()) { + if (!socket_cmd->Initialize()) { + return false; + } + } else { + return socket_cmd->IsSocketValid(); + } + + socket_cmd->SetNonblocking(); + + if (!socket_cmd->Open(lidarIP, tcpPort)) { + socket_cmd->Close(); + return false; + } + + socket_cmd->SetSendTimeout(DEFAULT_TIMEOUT / 1000, + (DEFAULT_TIMEOUT % 1000) * 1000); + socket_cmd->SetReceiveTimeout(DEFAULT_TIMEOUT / 1000, + (DEFAULT_TIMEOUT % 1000) * 1000); + socket_cmd->SetBlocking(); + + return socket_cmd->IsSocketValid(); +} + +void ETLidarDriver::disConfigConnect() { + ScopedLocker lock(_cmd_lock); + + if (!socket_cmd) { + return; + } + + socket_cmd->Close(); +} + +void ETLidarDriver::disconnect() { + isAutoReconnect = false; + stop(); + ScopedLocker lock(_data_lock); + + if (!socket_data) { + return; + } + + socket_data->Close(); +} + +std::string ETLidarDriver::getSDKVersion() { + return YDLIDAR_SDK_VERSION_STR; +} + +result_t ETLidarDriver::getHealth(device_health &health, uint32_t timeout) { + result_t ans = RESULT_OK; + disableDataGrabbing(); + + health.error_code = 0; + health.status = 0; + + return ans; +} + +result_t ETLidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) { + result_t ans = RESULT_OK; + + info.firmware_version = 0; + info.hardware_version = 0; + info.model = YDLIDAR_T15; + + return ans; +} + +result_t ETLidarDriver::startScan(bool force, uint32_t timeout) { + result_t ans; + + if (m_isScanning) { + return RESULT_OK; + } + + { + bool ret = startMeasure(); + + if (!ret) { + startMeasure(); + } + + if (!ret) { + return RESULT_FAIL; + } + + ans = this->createThread(); + return ans; + } + + return RESULT_OK; +} + + +result_t ETLidarDriver::stop() { + if (isAutoconnting) { + isAutoReconnect = false; + m_isScanning = false; + } + + disableDataGrabbing(); + + if (m_isValidDevice) { + bool ret = stopMeasure(); + + if (!ret) { + stopMeasure(); + } + } + + return RESULT_OK; +} + +result_t ETLidarDriver::createThread() { + m_isScanning = true; + _thread = CLASS_THREAD(ETLidarDriver, cacheScanData); + + if (_thread.getHandle() == 0) { + m_isScanning = false; + return RESULT_FAIL; + } + + return RESULT_OK; +} + + +char *ETLidarDriver::configMessage(const char *descriptor, char *value) { + char buf[100]; + char transDesc[32]; + char recvDesc[32]; + static char recvValue[32]; + + strncpy(transDesc, descriptor, sizeof(transDesc)); + valLastName(transDesc); + + sprintf(buf, "%s=%s\n", transDesc, value); + ScopedLocker lock(_cmd_lock); + + if (!socket_cmd) { + return NULL; + } + + socket_cmd->Send(reinterpret_cast(buf), strlen(buf)); + + memset(buf, 0, sizeof(buf)); + + if (socket_cmd->Select(0, 800000)) { + socket_cmd->Receive(sizeof(buf), reinterpret_cast(buf)); + + if (2 == sscanf(buf, "%[^=]=%[^=]", recvDesc, recvValue)) { + if (!strcmp(transDesc, recvDesc)) { + return recvValue; + } else { + return NULL; + } + } else { + return NULL; + } + + } else { + return value; + } + + return NULL; +} + +bool ETLidarDriver::startMeasure() { + bool ret; + + if (!configPortConnect(m_port.c_str(), port)) { + return false; + } + + lidarConfig cfg; + ret = (configMessage(valName(cfg.motor_en), (char *)configValue[1]) != NULL); + ret &= (configMessage(valName(cfg.laser_en), (char *)configValue[1]) != NULL); + disConfigConnect(); + return ret; +} + +bool ETLidarDriver::stopMeasure() { + if (!configPortConnect(m_port.c_str(), port)) { + return false; + } + + bool ret ; + lidarConfig cfg; + ret = (configMessage(valName(cfg.motor_en), (char *)configValue[0]) != NULL); + ret &= (configMessage(valName(cfg.laser_en), (char *)configValue[0]) != NULL); + disConfigConnect(); + return ret; +} + +lidarConfig ETLidarDriver::getFinishedScanCfg() { + return m_config; +} + +bool ETLidarDriver::getScanCfg(lidarConfig &config, + const std::string &ip_address) { + bool ret = true; + + if (!ip_address.empty()) { + m_port = ip_address; + } + + lidarConfig cfg; + + if (!configPortConnect(m_port.c_str(), port)) { + config = m_config; + ret = false; + return ret; + } + + char *result = configMessage(valName(cfg.laser_en)); + + if (result != NULL) { + cfg.laser_en = atoi(result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.motor_en)); + + if (result != NULL) { + cfg.motor_en = atoi(result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.motor_rpm)); + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.fov_start)); + + if (result != NULL) { + cfg.fov_start = atoi(result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.fov_end)); + + if (result != NULL) { + cfg.fov_end = atoi(result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.trans_sel)); + + if (result != NULL) { + cfg.trans_sel = atoi(result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.dataRecvPort)); + + if (result != NULL) { + cfg.dataRecvPort = atoi(result); + } else { + ret &= false; + } + + + result = configMessage(valName(cfg.dhcp_en)); + + if (result != NULL) { + cfg.dhcp_en = atoi(result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.dataRecvIp)); + + if (result != NULL) { + strcpy(cfg.dataRecvIp, result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.deviceIp)); + + if (result != NULL) { + strcpy(cfg.deviceIp, result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.deviceNetmask)); + + if (result != NULL) { + strcpy(cfg.deviceNetmask, result); + } else { + ret &= false; + } + + + result = configMessage(valName(cfg.deviceGatewayIp)); + + if (result != NULL) { + strcpy(cfg.deviceGatewayIp, result); + } else { + ret &= false; + } + + result = configMessage(valName(cfg.laserScanFrequency)); + + if (result != NULL) { + cfg.laserScanFrequency = atoi(result); + m_sampleRate = 1000 / cfg.laserScanFrequency * 1000; + } else { + cfg.laserScanFrequency = 50; + m_sampleRate = 1000 / cfg.laserScanFrequency * 1000; + } + + result = configMessage(valName(cfg.correction_angle)); + + if (result != NULL) { + cfg.correction_angle = atoi(result); + } else { + cfg.correction_angle = 20640; + } + + result = configMessage(valName(cfg.correction_distance)); + + if (result != NULL) { + cfg.correction_distance = atoi(result); + } else { + cfg.correction_distance = 6144; + + } + + if (ret) { + m_config = cfg; + config = cfg; + m_sampleRate = static_cast(1e9 / m_sampleRate); + } + + disConfigConnect(); + return ret; +} + + +void ETLidarDriver::setScanCfg(const lidarConfig &config) { + char str[32]; + + if (!configPortConnect(m_port.c_str(), port)) { + return ; + } + + char *result = NULL; + + if (m_config.motor_rpm != config.motor_rpm) { + _itoa(config.motor_rpm, str, 10); + result = configMessage(valName(config.motor_rpm), str); + + if (result != NULL) { + m_config.motor_rpm = atoi(result); + } + } + + + if (m_config.fov_start != config.fov_start) { + _itoa(config.fov_start, str, 10); + result = configMessage(valName(config.fov_start), str); + + if (result != NULL) { + m_config.fov_start = atoi(result); + } + } + + + if (m_config.fov_end != config.fov_end) { + _itoa(config.fov_end, str, 10); + result = configMessage(valName(config.fov_end), str); + + if (result != NULL) { + m_config.fov_end = atoi(result); + } + } + + + if (m_config.dataRecvPort != config.dataRecvPort) { + _itoa(config.dataRecvPort, str, 10); + result = configMessage(valName(config.dataRecvPort), str); + + if (result != NULL) { + m_config.dataRecvPort = atoi(result); + } + } + + + if (strcmp(m_config.dataRecvIp, config.dataRecvIp)) { + result = configMessage(valName(config.dataRecvIp), (char *)config.dataRecvIp); + + if (result != NULL) { + strcpy(m_config.dataRecvIp, result); + } + } + + disConfigConnect(); +} + + + +bool ETLidarDriver::dataPortConnect(const char *lidarIP, int localPort) { + ScopedLocker lock(_data_lock); + + if (!socket_data) { + return false; + } + + if (!socket_data->IsSocketValid()) { + if (socket_data->Initialize()) { + if (!socket_data->Listen(NULL, localPort)) { + socket_data->Close(); + return false; + } + + socket_data->SetReceiveTimeout(DEFAULT_TIMEOUT / 1000, + (DEFAULT_TIMEOUT % 1000) * 1000); + } + } + + return socket_data->IsSocketValid(); +} + +void ETLidarDriver::disableDataGrabbing() { + { + ScopedLocker l(_lock); + + if (m_isScanning) { + m_isScanning = false; + _dataEvent.set(); + } + } + _thread.join(); +} + +result_t ETLidarDriver::grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout) { + switch (_dataEvent.wait(timeout)) { + case Event::EVENT_TIMEOUT: + count = 0; + return RESULT_TIMEOUT; + + case Event::EVENT_OK: { + if (scan_node_count == 0) { + return RESULT_FAIL; + } + + ScopedLocker l(_lock); + size_t size_to_copy = std::min(count, scan_node_count); + memcpy(nodebuffer, scan_node_buf, size_to_copy * sizeof(node_info)); + count = size_to_copy; + scan_node_count = 0; + } + + return RESULT_OK; + + default: + count = 0; + return RESULT_FAIL; + } + +} + +result_t ETLidarDriver::getScanFrequency(scan_frequency &frequency, + uint32_t timeout) { + lidarConfig cfg; + result_t ans = RESULT_FAIL; + + if (!configPortConnect(m_port.c_str(), port)) { + return RESULT_FAIL; + } + + char *result = configMessage(valName(cfg.motor_rpm)); + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + frequency.frequency = static_cast(100 * (cfg.motor_rpm / 60.f + 0.4)); + ans = RESULT_OK; + } + + return ans; + +} + +result_t ETLidarDriver::setScanFrequencyAdd(scan_frequency &frequency, + uint32_t timeout) { + lidarConfig cfg; + result_t ans = RESULT_FAIL; + char str[32]; + + if (!configPortConnect(m_port.c_str(), port)) { + return ans; + } + + char *result = configMessage(valName(cfg.motor_rpm)); + + if (result == NULL) { + return ans; + } + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + + } + + cfg.motor_rpm += 60; + _itoa(cfg.motor_rpm, str, 10); + result = configMessage(valName(cfg.motor_rpm), str); + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + m_config.motor_rpm = cfg.motor_rpm; + frequency.frequency = static_cast(100 * (cfg.motor_rpm / 60.f + 0.4)); + ans = RESULT_OK; + } + + + return ans; +} + +result_t ETLidarDriver::setScanFrequencyDis(scan_frequency &frequency, + uint32_t timeout) { + lidarConfig cfg; + result_t ans = RESULT_FAIL; + char str[32]; + + if (!configPortConnect(m_port.c_str(), port)) { + return ans; + } + + char *result = configMessage(valName(cfg.motor_rpm)); + + if (result == NULL) { + return ans; + } + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + } + + cfg.motor_rpm -= 60; + _itoa(cfg.motor_rpm, str, 10); + result = configMessage(valName(cfg.motor_rpm), str); + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + m_config.motor_rpm = cfg.motor_rpm; + frequency.frequency = static_cast(100 * (cfg.motor_rpm / 60.f + 0.4)); + ans = RESULT_OK; + } + + + return ans; +} + +result_t ETLidarDriver::setScanFrequencyAddMic(scan_frequency &frequency, + uint32_t timeout) { + lidarConfig cfg; + result_t ans = RESULT_FAIL; + char str[32]; + + if (!configPortConnect(m_port.c_str(), port)) { + return ans; + } + + char *result = configMessage(valName(cfg.motor_rpm)); + + if (result == NULL) { + return ans; + } + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + + } + + cfg.motor_rpm += 6; + _itoa(cfg.motor_rpm, str, 10); + result = configMessage(valName(cfg.motor_rpm), str); + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + m_config.motor_rpm = cfg.motor_rpm; + frequency.frequency = static_cast(100 * (cfg.motor_rpm / 60.f + 0.4)); + ans = RESULT_OK; + } + + + return ans; + +} + +result_t ETLidarDriver::setScanFrequencyDisMic(scan_frequency &frequency, + uint32_t timeout) { + lidarConfig cfg; + result_t ans = RESULT_FAIL; + char str[32]; + + if (!configPortConnect(m_port.c_str(), port)) { + return ans; + } + + char *result = configMessage(valName(cfg.motor_rpm)); + + if (result == NULL) { + return ans; + } + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + + } + + cfg.motor_rpm -= 6; + _itoa(cfg.motor_rpm, str, 10); + result = configMessage(valName(cfg.motor_rpm), str); + + if (result != NULL) { + cfg.motor_rpm = atoi(result); + m_config.motor_rpm = cfg.motor_rpm; + frequency.frequency = static_cast(100 * (cfg.motor_rpm / 60.f + 0.4)); + ans = RESULT_OK; + } + + return ans; +} + +result_t ETLidarDriver::getSamplingRate(sampling_rate &rate, uint32_t timeout) { + + lidarConfig cfg; + result_t ans = RESULT_FAIL; + + if (!configPortConnect(m_port.c_str(), port)) { + return ans; + } + + char *result = configMessage(valName(cfg.laserScanFrequency)); + ans = RESULT_OK; + + if (result != NULL) { + cfg.laserScanFrequency = atoi(result); + m_sampleRate = 1000 / cfg.laserScanFrequency * 1000; + } else { + cfg.laserScanFrequency = 50; + m_sampleRate = 1000 / cfg.laserScanFrequency * 1000; + } + + rate.rate = YDLIDAR_RATE_10K; + + return ans; + +} + +result_t ETLidarDriver::setSamplingRate(sampling_rate &rate, uint32_t timeout) { + return RESULT_FAIL; +} + +result_t ETLidarDriver::getZeroOffsetAngle(offset_angle &angle, + uint32_t timeout) { + return RESULT_FAIL; +} + +result_t ETLidarDriver::setScanHeartbeat(scan_heart_beat &beat, + uint32_t timeout) { + return RESULT_FAIL; +} + +result_t ETLidarDriver::startAutoScan(bool force, uint32_t timeout) { + result_t ans; + { + bool ret = startMeasure(); + + if (!ret) { + startMeasure(); + } + + if (!ret) { + return RESULT_FAIL; + } + + } + + return RESULT_OK; +} + +result_t ETLidarDriver::checkAutoConnecting() { + result_t ans = RESULT_FAIL; + isAutoconnting = true; + m_InvalidNodeCount = 0; + setDriverError(TimeoutError); + + while (isAutoReconnect && isAutoconnting) { + { + disConfigConnect(); + ScopedLocker lock(_data_lock); + + if (!socket_data) { + return RESULT_FAIL; + } + + if (socket_data->isOpen()) { + size_t buffer_size = socket_data->available(); + + if (m_BufferSize && m_BufferSize % 90 == 0) { + setDriverError(BlockError); + } else { + if (buffer_size > 0 || m_BufferSize > 0) { + setDriverError(TrembleError); + m_BufferSize += buffer_size; + } else { + setDriverError(NotBufferError); + } + } + } + + socket_data->Close(); + } + retryCount++; + + if (retryCount > 100) { + retryCount = 100; + } + + int tempCount = 0; + + while (isAutoReconnect && isscanning() && tempCount < retryCount) { + delay(100); + tempCount++; + } + + tempCount = 0; + int retryConnect = 0; + + while (isAutoReconnect && + connect(m_port.c_str(), m_baudrate) != RESULT_OK) { + retryConnect++; + + if (retryConnect > 25) { + retryConnect = 25; + } + + setDriverError(NotOpenError); + + while (isAutoReconnect && isscanning() && tempCount < retryConnect) { + delay(200); + tempCount++; + } + } + + if (!isAutoReconnect) { + m_isScanning = false; + return RESULT_FAIL; + } + + if (isconnected()) { + delay(100); + { + ans = startAutoScan(); + + if (!IS_OK(ans)) { + ans = startAutoScan(); + } + } + + if (IS_OK(ans)) { + if (getDriverError() == DeviceNotFoundError) { + setDriverError(NoError); + } + + isAutoconnting = false; + return ans; + } else { + setDriverError(DeviceNotFoundError); + } + } + } + + return RESULT_FAIL; + +} + +void ETLidarDriver::CheckLaserStatus() { + if (m_InvalidNodeCount < 2) { + if (m_driverErrno == NoError) { + setDriverError(LaserFailureError); + } + } else { + if (m_driverErrno == LaserFailureError) { + setDriverError(NoError); + } + } + + m_InvalidNodeCount = 0; +} + +int ETLidarDriver::cacheScanData() { + node_info local_buf[100]; + size_t count = 100; + node_info local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + result_t ans = RESULT_FAIL; + memset(local_scan, 0, sizeof(local_scan)); + waitScanData(local_buf, count); + + int timeout_count = 0; + retryCount = 0; + m_BufferSize = 0; + m_InvalidNodeCount = 0; + bool m_last_frame_valid = false; + + while (m_isScanning) { + count = 100; + ans = waitScanData(local_buf, count); + + if (!IS_OK(ans)) { + if (IS_FAIL(ans) || timeout_count > DEFAULT_TIMEOUT_COUNT) { + if (!isAutoReconnect) { + fprintf(stderr, "exit scanning thread!!\n"); + fflush(stderr); + { + m_isScanning = false; + } + return RESULT_FAIL; + } else { + if (m_last_frame_valid) { + m_BufferSize = 0; + m_last_frame_valid = false; + } + + ans = checkAutoConnecting(); + + if (IS_OK(ans)) { + timeout_count = 0; + local_scan[0].sync = NODE_UNSYNC; + } else { + m_isScanning = false; + return RESULT_FAIL; + } + } + } else { + timeout_count++; + local_scan[0].sync = NODE_UNSYNC; + + if (m_driverErrno == NoError) { + setDriverError(TimeoutError); + } + + fprintf(stderr, "timeout count: %d\n", timeout_count); + fflush(stderr); + } + } else { + timeout_count = 0; + retryCount = 0; + m_BufferSize = 0; + m_last_frame_valid = true; + + if (retryCount != 0) { + setDriverError(NoError); + } + } + + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].sync & LIDAR_RESP_SYNCBIT) { + if ((local_scan[0].sync & LIDAR_RESP_SYNCBIT)) { + _lock.lock();//timeout lock, wait resource copy + local_scan[0].stamp = local_buf[pos].stamp; + local_scan[0].delayTime = local_buf[pos].delayTime; + local_scan[0].scanFreq = local_buf[pos].scanFreq; + memcpy(scan_node_buf, local_scan, scan_count * sizeof(node_info)); + scan_node_count = scan_count; + _dataEvent.set(); + _lock.unlock(); + } + + scan_count = 0; + } + + local_scan[scan_count++] = local_buf[pos]; + + if (scan_count == _countof(local_scan)) { + scan_count -= 1; + } + } + } + + m_isScanning = false; + + return RESULT_OK; +} + +result_t ETLidarDriver::waitScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout) { + if (!m_isConnected) { + count = 0; + return RESULT_FAIL; + } + + size_t recvNodeCount = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + result_t ans = RESULT_FAIL; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { + node_info node; + ans = waitPackage(&node, timeout - waitTime); + + if (!IS_OK(ans)) { + count = recvNodeCount; + return ans; + } + + nodebuffer[recvNodeCount++] = node; + + if (node.sync & LIDAR_RESP_SYNCBIT) { + count = recvNodeCount; + CheckLaserStatus(); + return RESULT_OK; + } + + if (recvNodeCount == count) { + return RESULT_OK; + } + } + + count = recvNodeCount; + return RESULT_FAIL; +} + +result_t ETLidarDriver::waitPackage(node_info *node, uint32_t timeout) { + + int offset; + result_t ans; + + if (nodeIndex == 0) { + ans = getScanData(); + + if (!IS_OK((ans))) { + return ans; + } + } + + (*node).sync = NODE_UNSYNC; + (*node).scanFreq = 0; + (*node).debugInfo = 0xff; + (*node).index = 0xff; + + offset = frame.dataIndex + 4 * nodeIndex; + (*node).dist = static_cast(DSL(frame.frameBuf[offset + 2], + 8) | DSL(frame.frameBuf[offset + 3], 0)); + + if ((*node).dist > 0) { + m_InvalidNodeCount++; + } + + if (isV1Protocol(frame.dataFormat)) { + (*node).qual = (uint16_t)(DSL(frame.frameBuf[offset], + 8) | DSL(frame.frameBuf[offset + 1], 0)); + } else { + (*node).qual = (uint16_t)frame.frameBuf[offset]; + } + + if (nodeIndex > 0) { + if (isV1Protocol(frame.dataFormat)) { + m_currentAngle = (frame.frameCrc - frame.startAngle) / (frame.dataNum - 1) / + 100.f; + } else { + m_currentAngle = frame.frameBuf[offset + 1] / 100.f; + } + + m_currentAngle += m_lastAngle; + } else { + m_currentAngle = frame.startAngle / 100.f; + } + + m_currentAngle = ydlidar::core::math::normalize_angle_positive_from_degree( + m_currentAngle); + (*node).angle = static_cast(m_currentAngle * 100); + m_lastAngle = m_currentAngle; + nodeIndex++; + + if (nodeIndex >= frame.dataNum) { + (*node).sync = frame.headFrameFlag ? NODE_SYNC : NODE_UNSYNC; + (*node).stamp = getTime();//(uint64_t)(frame.timestamp * 100); + (*node).delayTime = 0; + nodeIndex = 0; + m_lastAngle = 0.f; + m_currentAngle = 0.f; + } + + return RESULT_OK; +} + +result_t ETLidarDriver::getScanData() { + /* wait data from socket. */ + { + ScopedLocker lock(_data_lock); + + if (!socket_data) { + return RESULT_FAIL; + } + + if (socket_data->Receive(sizeof(frame.frameBuf), + reinterpret_cast(frame.frameBuf)) < 0) { + return RESULT_TIMEOUT; + } + } + + /* check frame head */ + frame.frameHead = DSL(frame.frameBuf[0], 8) | DSL(frame.frameBuf[1], 0); + + if (FRAME_PREAMBLE != frame.frameHead) { + return RESULT_TIMEOUT; + } + + /* check device type */ + frame.deviceType = (frame.frameBuf[2] >> 4) & 0xf; + + if (LIDAR_2D != frame.deviceType) { + return RESULT_TIMEOUT; + } + + /* check frame type */ + frame.frameType = frame.frameBuf[2] & 0xf; + + if (DATA_FRAME != frame.frameType) { + return RESULT_TIMEOUT; + } + + /* parser head length */ + frame.dataIndex = (frame.frameBuf[3] >> 4) & 0xf; + frame.dataIndex = (frame.dataIndex + 1) * 4; + + /* parser frame index */ + frame.frameIndex = frame.frameBuf[3] & 0xf; + + /* parser timestamp */ + frame.timestamp = DSL(frame.frameBuf[4], 24) | DSL(frame.frameBuf[5], 16) + | DSL(frame.frameBuf[6], 8) | DSL(frame.frameBuf[7], 0); + + /* parser head frame flag */ + frame.headFrameFlag = (frame.frameBuf[8] >> 4) & 0xf; + + /* parser data format */ + frame.dataFormat = frame.frameBuf[8] & 0xf; + + /* parser distance scale */ + frame.disScale = frame.frameBuf[9]; + + /* parser start angle */ + frame.startAngle = DSL(frame.frameBuf[10], 8) | DSL(frame.frameBuf[11], 0); + + /* parser valid data number */ + frame.dataNum = DSL(frame.frameBuf[12], 8) | DSL(frame.frameBuf[13], 0); + + /* parser frame crc */ + frame.frameCrc = DSL(frame.frameBuf[14], 8) | DSL(frame.frameBuf[15], 0); + + if (frame.dataNum < 1) { + return RESULT_TIMEOUT; + } + + return RESULT_OK; + +} + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.h new file mode 100644 index 0000000..9e6c1bb --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ETLidarDriver.h @@ -0,0 +1,454 @@ +/***************************************************************************** +* EAI TOF LIDAR DRIVER * +* Copyright (C) 2018-2020 EAI TEAM chushuifurong618@eaibot.com. * +* * +* This file is part of EAI TOF LIDAR DRIVER. * +* * +* @file ETLidarDriver.h * +* @brief TOF LIDAR DRIVER * +* Details. * +* * +* @author Tony.Yang * +* @email chushuifurong618@eaibot.com * +* @version 1.0.0(Version) * +* @date chushuifurong618@eaibot.com * +* * +* * +*----------------------------------------------------------------------------* +* Remark : Description * +*----------------------------------------------------------------------------* +* Change History : * +* | | | * +*----------------------------------------------------------------------------* +* 2018/08/09 | 1.0.0 | Tony.Yang | Create file * +*----------------------------------------------------------------------------* +* * +*****************************************************************************/ + +/** @page ETLidarDriver + * ETLidarDriver API + +
Library ETLidarDriver +
File ETLidarDriver.h +
Author Tony [code at ydlidar com] +
Source https://github.com/ydlidar/YDLidar-SDK +
Version 1.0.0 +
+ This ETLidarDriver support [TYPE_TOF_NET](\ref LidarTypeID::TYPE_TOF_NET) LiDAR + +* @copyright Copyright (c) 2018-2020 EAIBOT + Jump to the @link ::ydlidar::ETLidarDriver @endlink interface documentation. +*/ + +#pragma once + +/* Header file to enable threading and ergo callback */ +#include +#include +#include +#include +#include +#include +/* Header files for socket variable */ + + +namespace ydlidar { +namespace core { +namespace network { +class CActiveSocket; +class CPassiveSocket; +} +} + +using namespace core::common; +using namespace core::base; + + +class ETLidarDriver : public DriverInterface { + public: + + /** + * @brief ETLidarDriver + * @param lidarIP + * @param port + */ + explicit ETLidarDriver(); + + ~ETLidarDriver(); + + + /** + * @brief Connecting Lidar \n + * After the connection if successful, you must use ::disconnect to close + * @param[in] port_path Ip Address + * @param[in] baudrate network port + * @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); + + + /** + * @brief Returns a human-readable description of the given error code + * or the last error code of a socket + * @param isTCP TCP or UDP + * @return error information + */ + virtual const char *DescribeError(bool isTCP = true); + + /** + * @brief Disconnect from ETLidar device. + */ + virtual void disconnect(); + + /** + * @brief Get SDK Version + * @return version + */ + virtual std::string getSDKVersion(); + + /** + * @brief Is the Lidar in the scan \n + * @return scanning status + * @retval true scanning + * @retval false non-scanning + */ + virtual bool isscanning() const; + + /** + * @brief Is it connected to the lidar \n + * @return connection status + * @retval true connected + * @retval false Non-connected + */ + virtual bool isconnected() const; + + /** + * @brief Is there intensity \n + * @param[in] isintensities intentsity + * true intensity + * false no intensity + */ + virtual void setIntensities(const bool &isintensities); + + /** + * @brief whether to support hot plug \n + * @param[in] enable hot plug : + * true support + * false no support + */ + virtual void setAutoReconnect(const bool &enable); + + /** + * @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); + + /** + * @brief get Device information \n + * @param[in] info 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 &info, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @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) ; + + /** + * @brief turn off scanning \n + * @return result status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + virtual result_t stop(); + + + /** + * @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) ; + + /** + * @brief Get lidar scan frequency \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 getScanFrequency(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + /** + * @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); + + /** + * @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); + + /** + * @brief Get current scan configuration. + * @returns scanCfg structure. + */ + bool getScanCfg(lidarConfig &config, const std::string &ip_address = ""); + + /** + * @brief Get current scan update configuration. + * @returns scanCfg structure. + */ + lidarConfig getFinishedScanCfg(); + + /** + * @brief updateScanCfg + * @param config + */ + void updateScanCfg(const lidarConfig &config); + + private: + /** + * @brief Connect config port to ETLidar. + * @param remote IP & port. + */ + bool configPortConnect(const char *lidarIP, int tcpPort = 9000); + + /** + * @brief disConfigConnect + */ + void disConfigConnect(); + + /** + * @brief Set scan configuration. + * @param cfg structure containing scan configuration. + */ + void setScanCfg(const lidarConfig &config); + + + /** + * @brief Disconnect from ETLidar device. + */ + char *configMessage(const char *descriptor, char *value = NULL); + + /** + * @brief Start measurements. + * After receiving this command ETLidar unit starts spinning laser and measuring. + */ + bool startMeasure(); + + /** + * @brief Stop measurements. + * After receiving this command ETLidar unit stop spinning laser and measuring. + */ + bool stopMeasure(); + + /** + * @brief Connect data port to ETLidar. + * @param remote IP & local port. + */ + bool dataPortConnect(const char *lidarIP, int localPort = 8000); + + /** + * @brief createThread + * @return + */ + result_t createThread(); + + /** + * @brief disableDataGrabbing + */ + void disableDataGrabbing(); + /** + * @brief Receive scan message. + * + * @param data pointer to lidarData buffer structure. + */ + result_t getScanData(); + + /** + * @brief Turn on Lidar in Scanning thread \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 + */ + result_t startAutoScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT) ; + + + /** + * @brief checkAutoConnecting + * @return + */ + result_t checkAutoConnecting(); + + /** + * @brief CheckLaserStatus + */ + void CheckLaserStatus(); + + /** + * @brief parsing scan \n + */ + int cacheScanData(); + + /** + * @brief get unpacked data \n + * @param[in] nodebuffer laser node + * @param[in] count lidar points size + * @param[in] timeout timeout + * @return result status + * @retval RESULT_OK success + * @retval RESULT_TIMEOUT timeout + * @retval RESULT_FAILE failed + */ + result_t waitScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief Unpacking \n + * @param[in] node lidar point information + * @param[in] timeout timeout + */ + result_t waitPackage(node_info *node, uint32_t timeout = DEFAULT_TIMEOUT); + + + private: + /* Variable for LIDAR compatibility */ + Locker _data_lock; ///< + lidarConfig m_user_config; + size_t offset_len; + int m_AbnormalCheckCount; + bool m_force_update; + float m_lastAngle; + float m_currentAngle; + /* ETLidar specific Variables */ + std::string m_deviceIp; + int port; + int m_sampleRate; + /* Sockets for ydlidar */ + core::network::CActiveSocket *socket_cmd; + core::network::CPassiveSocket *socket_data; + dataFrame frame; + const char *configValue[2] = {"0", "1"}; + bool m_isValidDevice; + +}; + +} /* namespace */ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.cpp new file mode 100644 index 0000000..0984e91 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.cpp @@ -0,0 +1,2030 @@ +/********************************************************************* +* 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. +*********************************************************************/ +#include +#include +#include +#include "GSLidarDriver.h" +#include "core/serial/common.h" +#include "core/serial/serial.h" +#include "core/network/ActiveSocket.h" +#include "core/common/ydlidar_help.h" +#include "ydlidar_config.h" + +#define GS_CMD_STARTIAP 0x0A //启动IAP +#define GS_CMD_EXECIAP 0x0B //运行IAP，传输数据包 +#define GS_CMD_STOPIAP 0x0C //停止IAP +#define GS_CMD_ACKIAP 0x20 //IAP应答 +#define GS_CMD_RESET 0x67 //复位 +#define GS_CMD_ACKOK 0x01 //正常 +#define GS_CMD_ZERO 0x00 //0 + +#define DATA_LEN_PER_FRAME (81 - 18 + 1) //每帧数据长度 + +using namespace impl; + +namespace ydlidar { +using namespace core::common; +using namespace core::serial; +using namespace core::network; + +GSLidarDriver::GSLidarDriver(uint8_t type) +{ + //串口配置参数 + m_intensities = false; + isAutoReconnect = true; + m_baudrate = 230400; + scan_node_count = 0; + sample_rate = 5000; + m_PointTime = 1e9 / 5000; + trans_delay = 0; + retryCount = 0; + m_SingleChannel = false; + m_LidarType = TYPE_GS; + m_DeviceType = type; + //解析参数 + PackageSampleBytes = 2; + CheckSum = 0; + CheckSumCal = 0; + CheckSumResult = false; + moduleNum = 0; + + nodeIndex = 0; + globalRecvBuffer = new uint8_t[GSPACKSIZE]; + scan_node_buf = new node_info[MAX_SCAN_NODES]; + for (int i=0; iisOpen()) { + _comm->flush(); + _comm->closePort(); + } + delete _comm; + _comm = NULL; + } + + if (globalRecvBuffer) { + delete[] globalRecvBuffer; + globalRecvBuffer = NULL; + } + if (scan_node_buf) { + delete[] scan_node_buf; + scan_node_buf = NULL; + } +} + +result_t GSLidarDriver::connect(const char *port_path, uint32_t baudrate) +{ + m_baudrate = baudrate; + m_port = string(port_path); + { + ScopedLocker l(_cmd_lock); + if (!_comm) + { + if (m_DeviceType == YDLIDAR_TYPE_TCP) + { + _comm = new CActiveSocket(); + } + else + { + _comm = new serial::Serial(m_port, m_baudrate, + serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT)); + } + _comm->bindport(port_path, baudrate); + } + if (!_comm->open()) + { + setDriverError(NotOpenError); + return RESULT_FAIL; + } + m_isConnected = true; + } + + stopScan(); + // delay(100); + // clearDTR(); + //配置GS2模组地址（三个模组） + setDeviceAddress(300); + + return RESULT_OK; +} + +void GSLidarDriver::setDTR() { + if (!m_isConnected) { + return ; + } + + if (_comm) { + _comm->setDTR(1); + } + +} + +void GSLidarDriver::clearDTR() { + if (!m_isConnected) { + return ; + } + + if (_comm) { + _comm->setDTR(0); + } +} + +void GSLidarDriver::flushSerial() { + if (!m_isConnected) { + return; + } + + size_t len = _comm->available(); + if (len) { + _comm->readSize(len); + } + + delay(20); +} + +void GSLidarDriver::disconnect() { + isAutoReconnect = false; + + if (!m_isConnected) { + return ; + } + + stop(); + delay(10); + ScopedLocker l(_cmd_lock); + + if (_comm) { + if (_comm->isOpen()) { + _comm->closePort(); + } + } + + m_isConnected = false; +} + +void GSLidarDriver::disableDataGrabbing() +{ + if (m_isScanning) { + m_isScanning = false; + _dataEvent.set(); + } + if (m_thread) + { + if (m_thread->joinable()) + m_thread->join(); + delete m_thread; + m_thread = nullptr; + } + // _thread.join(); +} + +bool GSLidarDriver::isscanning() const { + return m_isScanning; +} +bool GSLidarDriver::isconnected() const { + return m_isConnected; +} + +result_t GSLidarDriver::sendCommand(uint8_t cmd, + const void *payload, + size_t payloadsize) +{ + return sendCommand(0x00, cmd, payload, payloadsize); +// uint8_t pkt_header[12]; +// gs_package_head *header = reinterpret_cast(pkt_header); +// uint8_t checksum = 0; + +// if (!m_isConnected) { +// return RESULT_FAIL; +// } + +// header->syncByte0 = LIDAR_CMD_SYNC_BYTE; +// header->syncByte1 = LIDAR_CMD_SYNC_BYTE; +// header->syncByte2 = LIDAR_CMD_SYNC_BYTE; +// header->syncByte3 = LIDAR_CMD_SYNC_BYTE; +// header->address = 0x00; +// header->cmd_flag = cmd; +// header->size = 0xffff&payloadsize; +// sendData(pkt_header, 8) ; +// checksum += cmd; +// checksum += 0xff&header->size; +// checksum += 0xff&(header->size>>8); + +// if (payloadsize && payload) { +// for (size_t pos = 0; pos < payloadsize; ++pos) { +// checksum += ((uint8_t *)payload)[pos]; +// } +// uint8_t sizebyte = (uint8_t)(payloadsize); +// sendData((const uint8_t *)payload, sizebyte); +// } + +// sendData(&checksum, 1); + +// return RESULT_OK; +} + +result_t GSLidarDriver::sendCommand(uint8_t addr, + uint8_t cmd, + const void *payload, + size_t payloadsize) +{ + uint8_t pkt_header[12]; + gs_package_head *header = reinterpret_cast(pkt_header); + uint8_t checksum = 0; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + header->syncByte0 = LIDAR_CMD_SYNC_BYTE; + header->syncByte1 = LIDAR_CMD_SYNC_BYTE; + header->syncByte2 = LIDAR_CMD_SYNC_BYTE; + header->syncByte3 = LIDAR_CMD_SYNC_BYTE; + header->address = addr; + header->type = cmd; + header->size = 0xffff&payloadsize; + sendData(pkt_header, 8) ; + checksum += addr; + checksum += cmd; + checksum += 0xff&header->size; + checksum += 0xff&(header->size>>8); + + if (payloadsize && payload) { + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum += ((uint8_t *)payload)[pos]; + } + uint8_t sizebyte = (uint8_t)(payloadsize); + sendData((const uint8_t *)payload, sizebyte); + } + + sendData(&checksum, 1); + + return RESULT_OK; +} + +result_t GSLidarDriver::sendData(const uint8_t *data, size_t size) { + if (!_comm || !_comm->isOpen()) { + return RESULT_FAIL; + } + + if (data == NULL || size == 0) { + return RESULT_FAIL; + } + + size_t r; + + while (size) + { + r = _comm->writeData(data, size); + if (!r) + { + return RESULT_FAIL; + } + + if (m_Debug) + { + debugh(data, r); + } + + size -= r; + data += r; + } + + return RESULT_OK; +} + +result_t GSLidarDriver::getData(uint8_t *data, size_t size) { + if (!_comm || !_comm->isOpen()) { + return RESULT_FAIL; + } + + size_t r; + while (size) + { + r = _comm->readData(data, size); + if (!r) + { + return RESULT_FAIL; + } + + if (m_Debug) + { + debugh(data, r); + } + + size -= r; + data += r; + } + + return RESULT_OK; +} + +result_t GSLidarDriver::waitResponseHeader(gs_package_head *header, + uint32_t timeout) { + int recvPos = 0; + uint32_t startTs = getms(); + uint8_t recvBuffer[sizeof(gs_package_head)]; + uint8_t *headerBuffer = reinterpret_cast(header); + uint32_t waitTime = 0; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(gs_package_head) - recvPos; + size_t recvSize = 0; + result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + + if (!IS_OK(ans)) { + return ans; + } + + if (recvSize > remainSize) { + recvSize = remainSize; + } + + ans = getData(recvBuffer, recvSize); + + if (IS_FAIL(ans)) { + return RESULT_FAIL; + } + + for (size_t pos = 0; pos < recvSize; ++pos) { + uint8_t currentByte = recvBuffer[pos]; + + switch (recvPos) { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 2: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 3: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + default: + break; + } + + headerBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(gs_package_head)) { + return RESULT_OK; + } + } + } + + return RESULT_FAIL; +} + +result_t GSLidarDriver::waitResponseHeaderEx( + gs_package_head *header, + uint8_t cmd, + uint32_t timeout) +{ + int recvPos = 0; + uint32_t startTs = getms(); + uint8_t recvBuffer[GSPACKEGEHEADSIZE]; + uint8_t *headerBuffer = reinterpret_cast(header); + uint32_t waitTime = 0; + + while ((waitTime = getms() - startTs) <= timeout) + { + size_t remainSize = GSPACKEGEHEADSIZE - recvPos; + size_t recvSize = 0; + result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + if (!IS_OK(ans)) { + return ans; + } + + if (recvSize > remainSize) { + recvSize = remainSize; + } + + ans = getData(recvBuffer, recvSize); + if (IS_FAIL(ans)) { + return RESULT_FAIL; + } + + for (size_t pos = 0; pos < recvSize; ++pos) + { + uint8_t currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + case 2: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + case 3: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + case 4: + if (currentByte == LIDAR_ANS_SYNC_BYTE1) + continue; + break; + case 5: + if (currentByte != cmd) { + recvPos = 0; + continue; + } + break; + default: + break; + } + + headerBuffer[recvPos++] = currentByte; + + if (recvPos == GSPACKEGEHEADSIZE) { + return RESULT_OK; + } + } + } + + return RESULT_FAIL; +} + +result_t GSLidarDriver::waitForData(size_t data_count, uint32_t timeout, + size_t *returned_size) { + size_t length = 0; + + if (returned_size == NULL) { + returned_size = (size_t *)&length; + } + + return (result_t)_comm->waitfordata(data_count, timeout, returned_size); +} + +result_t GSLidarDriver::checkAutoConnecting() +{ + result_t ans = RESULT_FAIL; + + if (m_driverErrno != BlockError) + setDriverError(TimeoutError); + + while (isAutoReconnect && isscanning()) + { + { + ScopedLocker l(_cmd_lock); + if (_comm) { + if (_comm->isOpen()) { + m_isConnected = false; + _comm->closePort(); + } + } + } + delay(100); //延时 + + while (isscanning() && + connect(m_port.c_str(), m_baudrate) != RESULT_OK) + { + setDriverError(NotOpenError); + delay(300); + } + + if (!isscanning()) { + return RESULT_FAIL; + } + //判断是否已重连，如是则尝试启动雷达 + if (isconnected()) + { + delay(100); + ans = startAutoScan(); + if (IS_OK(ans)) { + return ans; + } + else { + setDriverError(DeviceNotFoundError); + } + } + } + + return RESULT_FAIL; +} + +int GSLidarDriver::cacheScanData() +{ + node_info local_buf[GS_PACKMAXNODES]; + size_t count = GS_PACKMAXNODES; + size_t scan_count = 0; + result_t ans = RESULT_FAIL; + + int timeout_count = 0; + retryCount = 0; + lastStamp = 0; + + m_isScanning = true; + + while (m_isScanning) + { + count = GS_PACKMAXNODES; + ans = waitScanData(local_buf, count); + // Thread::needExit(); + if (!IS_OK(ans)) + { + if (IS_FAIL(ans)) + { + timeout_count ++; + } + else + { + timeout_count += 2; + if (m_driverErrno != BlockError) + setDriverError(TimeoutError); + } + fprintf(stderr, "[GSLIDAR] Timeout count: %d\n", timeout_count); + fflush(stderr); + // 重连雷达 + if (!isAutoReconnect) + { + fprintf(stderr, "[GSLIDAR] Exit scanning thread\n"); + fflush(stderr); + m_isScanning = false; + return RESULT_FAIL; + } + else if (timeout_count > DEFAULT_TIMEOUT_COUNT) + { + ans = checkAutoConnecting(); + if (IS_OK(ans)) + { + timeout_count = 0; + } + else + { + m_isScanning = false; + return RESULT_FAIL; + } + } + } + else + { + timeout_count = 0; + retryCount = 0; + + { + //数据存入数组 + ScopedLocker l(_lock); + gs_module_nodes nodes; + nodes.moduleNum = moduleNum; + nodes.pointCount = count; + memcpy(nodes.points, local_buf, count * SDKNODESIZE); + datas.push_back(nodes); + scan_count = 0; + } + } + } + + m_isScanning = false; + + return RESULT_OK; +} + +result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) +{ + int pos = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + uint8_t *packageBuffer = (uint8_t *)&package; + int package_recvPos = 0; + uint16_t sample_lens = 0; + uint16_t package_Sample_Num = 0; + result_t ret = RESULT_FAIL; + size_t recvSize = 0; + size_t remainSize = 0; + CheckSumCal = 0; + + if (nodeIndex == 0) + { + pos = 0; + while ((waitTime = getms() - startTs) < timeout) + { + //解析协议头部分 + remainSize = GS_PACKHEADSIZE - pos; + recvSize = 0; + ret = waitForData(remainSize, timeout - waitTime, &recvSize); + if (!IS_OK(ret)) + return ret; + if (recvSize > remainSize) + recvSize = remainSize; + getData(globalRecvBuffer, recvSize); + +PARSEHEAD: + for (size_t i = 0; i < recvSize; ++i) + { + uint8_t c = globalRecvBuffer[i]; + switch (pos) + { + case 0: + if (c != LIDAR_ANS_SYNC_BYTE1) + { + pos = 0; + continue; + } + break; + case 1: + if (c != LIDAR_ANS_SYNC_BYTE1) + { + pos = 0; + continue; + } + break; + case 2: + if (c != LIDAR_ANS_SYNC_BYTE1) + { + pos = 0; + continue; + } + break; + case 3: + if (c != LIDAR_ANS_SYNC_BYTE1) + { + pos = 0; + continue; + } + break; + case 4: + if (c == LIDAR_ANS_SYNC_BYTE1) //过滤出现超过4个包头标识的情况 + continue; + moduleNum = uint8_t(c >> 1); //模组地址转编号: 1, 2, 4 + CheckSumCal = c; + break; + case 5: + if (c != GS_LIDAR_ANS_SCAN) + { + pos = 0; + CheckSumCal = 0; + moduleNum = 0; + continue; + } + CheckSumCal += c; + break; + case 6: + sample_lens |= 0x00ff & c; + CheckSumCal += c; + break; + case 7: + sample_lens |= (0x00ff & c) << 8; + CheckSumCal += c; + break; + default: + break; + } + + packageBuffer[pos++] = c; + + // 如果解析到协议头 + if (pos == GS_PACKHEADSIZE) + { + // 如果协议数据长度不对则跳过，继续解析协议头 + if (!sample_lens || sample_lens >= GSPACKSIZE) + { + moduleNum = 0; + pos = 0; + continue; + } + package_Sample_Num = sample_lens + 1; // 环境2Bytes + 点云320Bytes + CRC + package_recvPos = pos; + nodeCount = (sample_lens - 2) / GSNODESIZE; //计算1包数据中的点数 + // info("Sample num %d", (package_Sample_Num - 3) / 2); + pos = 0; + // 解析协议数据部分 + while ((waitTime = getms() - startTs) <= timeout) + { + int offset = 0; // 缓存偏移量 + // 如果解析协议头时接收数据长度超过定义的长度则认为是从校验和错误处跳转过来的 + if (recvSize > GS_PACKHEADSIZE) + { + offset = i + 1; + } + else + { + remainSize = package_Sample_Num - pos; + recvSize = 0; + ret = waitForData(remainSize, timeout - waitTime, &recvSize); + if (!IS_OK(ret)) + return ret; + if (recvSize > remainSize) + recvSize = remainSize; + getData(globalRecvBuffer, recvSize); + } + + for (size_t j = offset; j < recvSize; ++j) + { + if (pos + 1 == package_Sample_Num) + { + CheckSum = globalRecvBuffer[recvSize - 1]; // crc + packageBuffer[package_recvPos + pos] = CheckSum; // crc + pos ++; + break; + } + + CheckSumCal += globalRecvBuffer[j]; + packageBuffer[package_recvPos + pos] = globalRecvBuffer[j]; + pos ++; + } + + if (pos == package_Sample_Num) + { + pos = 0; + // 判断校验和是否一致 + if (CheckSumCal != CheckSum) + { + CheckSumResult = false; + error("GS cs 0x%02X != 0x%02X", CheckSumCal, CheckSum); + // 如果校验和不一致，则需要跳转去当前缓存中查找协议头， + // 以免因当前数据包有缺失导致下一包数据解析失败 + goto PARSEHEAD; + } + else + { + CheckSumResult = true; + if (lastStamp > 0) + { + m_ScanFreq = 1000.0 / (getms() - lastStamp); + } + lastStamp = getms(); + } + break; + } + recvSize = 0; //重置缓存数据大小 + } + + break; + } // end if (pos == GS_PACKHEADSIZE) + } //end for (size_t i = 0; i < recvSize; ++i) + if (CheckSumResult) + break; + } //end while ((waitTime = getms() - startTs) <= timeout) + + if (CheckSumResult) + { + model = m_models[moduleNum]; //当前雷达型号 + if (m_Debug) + debug("GS lidar module[%d] model[%u]", moduleNum, model); + //根据雷达型号设置角度参数 + if (YDLIDAR_GS5 == model) + m_pitchAngle = Angle_PAngle2; + else + m_pitchAngle = Angle_PAngle; + } + } //end if (nodeIndex == 0) + + (*node).stamp = getTime(); + + if (CheckSumResult) + { + //第1个点时间戳使用上一帧的最后1个点的时间戳 + if (nodeIndex == 0) + (*node).stamp = stamp ? stamp : getTime(); + else + (*node).stamp = getTime(); + stamp = (*node).stamp; + + (*node).index = moduleNum; + (*node).scanFreq = m_ScanFreq; + (*node).qual = 0; + (*node).sync = NODE_UNSYNC; + + if (YDLIDAR_GS1 == model) + { + //GS1低10位为距离，高6位为信号强度 + (*node).dist = uint16_t(package.nodes[nodeIndex].node & 0x03FF); + //如果配置了信号强度则处理信号强度 + if (m_intensities) + (*node).qual = uint16_t(package.nodes[nodeIndex].node >> 10); + } + else if (YDLIDAR_GS5 == model || + YDLIDAR_GS6 == model) + { + //GS5、GS6低11位为距离，高5位为信号强度 + (*node).dist = uint16_t(package.nodes[nodeIndex].node & 0x07FF); + //如果配置了信号强度则处理信号强度 + if (m_intensities) + (*node).qual = uint16_t(package.nodes[nodeIndex].node >> 11); + } + else + { + //GS2低9位为距离，高7位为信号强度 + (*node).dist = uint16_t(package.nodes[nodeIndex].node & 0x01FF); + //如果配置了信号强度则处理信号强度 + if (m_intensities) + (*node).qual = uint16_t(package.nodes[nodeIndex].node >> 9); + } + + double sampleAngle = 0; + if (node->dist > 0) + { + if (YDLIDAR_GS1 == model) + angTransform2((*node).dist, nodeIndex, + &sampleAngle, &(*node).dist); + else if (YDLIDAR_GS6 == model) + angTransform2((*node).dist, nodeCount - nodeIndex, + &sampleAngle, &(*node).dist); + else + angTransform((*node).dist, nodeIndex, + &sampleAngle, &(*node).dist); + } + + if (sampleAngle < 0) + { + (*node).angle = (((uint16_t)(sampleAngle * 64 + 23040)) << LIDAR_RESP_ANGLE_SHIFT) + + LIDAR_RESP_CHECKBIT; + } + else + { + if ((sampleAngle * 64) > 23040) + { + (*node).angle = (((uint16_t)(sampleAngle * 64 - 23040)) << LIDAR_RESP_ANGLE_SHIFT) + + LIDAR_RESP_CHECKBIT; + } + else + { + (*node).angle = (((uint16_t)(sampleAngle * 64)) << LIDAR_RESP_ANGLE_SHIFT) + + LIDAR_RESP_CHECKBIT; + } + } + + if (YDLIDAR_GS2 == model || + YDLIDAR_GS5 == model) + { + // 过滤左右相机超过0°的点 + if (nodeIndex < 80) + { // CT_RingStart CT_Normal + if ((*node).angle <= 23041) + { + (*node).dist = 0; + } + } + else + { + if ((*node).angle > 23041) + { + (*node).dist = 0; + } + } + } + + //处理环境数据（2个字节分别存储在两个点的is属性中） + if (0 == nodeIndex) + (*node).is = package.env & 0xFF; + else if (1 == nodeIndex) + (*node).is = package.env >> 8; + + // debug("%u 0x%X %.02f %.02f", nodeIndex, + // package.nodes[nodeIndex].dist, + // sampleAngle, node->dist/1.0); + } + else + { + (*node).qual = 0; + (*node).angle = LIDAR_RESP_CHECKBIT; + (*node).dist = 0; + (*node).scanFreq = 0; + return RESULT_FAIL; + } + + nodeIndex ++; + if (nodeIndex >= nodeCount) + { + nodeIndex = 0; + (*node).sync = NODE_SYNC; + CheckSumResult = false; + } + + return RESULT_OK; +} + +void GSLidarDriver::angTransform( + uint16_t dist, + int n, + double *dstTheta, + uint16_t *dstDist) +{ + double pixelU = n, Dist, theta, tempTheta, tempDist, tempX, tempY; + uint8_t mdNum = moduleNum; + if (n < nodeCount / 2) + { + pixelU = nodeCount / 2 - pixelU; + if (b0[mdNum] > 1) { + tempTheta = k0[mdNum] * pixelU - b0[mdNum]; + } + else + { + tempTheta = atan(k0[mdNum] * pixelU - b0[mdNum]) * 180 / M_PI; + } + tempDist = (dist - Angle_Px) / cos(((m_pitchAngle + bias[mdNum]) - (tempTheta)) * M_PI / 180); + tempTheta = tempTheta * M_PI / 180; + tempX = cos((m_pitchAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + + sin((m_pitchAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta)); + tempY = -sin((m_pitchAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + + cos((m_pitchAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta)); + tempX = tempX + Angle_Px; + tempY = tempY - Angle_Py; //5.315 + Dist = sqrt(tempX * tempX + tempY * tempY); + theta = atan(tempY / tempX) * 180 / M_PI; + } + else + { + pixelU = nodeCount - pixelU; + if (b1[mdNum] > 1) + { + tempTheta = k1[mdNum] * pixelU - b1[mdNum]; + } + else + { + tempTheta = atan(k1[mdNum] * pixelU - b1[mdNum]) * 180 / M_PI; + } + tempDist = (dist - Angle_Px) / cos(((m_pitchAngle + bias[mdNum]) + (tempTheta)) * M_PI / 180); + tempTheta = tempTheta * M_PI / 180; + tempX = cos(-(m_pitchAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + + sin(-(m_pitchAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta)); + tempY = -sin(-(m_pitchAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + + cos(-(m_pitchAngle + bias[mdNum]) * M_PI / 180) * (tempDist * sin(tempTheta)); + tempX = tempX + Angle_Px; + tempY = tempY + Angle_Py; //5.315 + Dist = sqrt(tempX * tempX + tempY * tempY); + theta = atan(tempY / tempX) * 180 / M_PI; + } + if (theta < 0) + { + theta += 360; + } + *dstTheta = theta; + *dstDist = Dist; + + // debug("%d %d %f %d", n, dist, (float)theta, (int)Dist); +} + +void GSLidarDriver::angTransform2( + uint16_t dist, + int n, + double *dstTheta, + uint16_t *dstDist) +{ + double pixelU = nodeCount - n, Dist, theta, tempTheta; + uint8_t mdNum = moduleNum; + + tempTheta = atan(k0[mdNum] * pixelU - b0[mdNum]) * 180 / M_PI; + Dist = dist / cos(tempTheta * M_PI / 180); + theta = tempTheta; + + if (theta < 0) + { + theta += 360; + } + *dstTheta = theta; + *dstDist = Dist; +} + +result_t GSLidarDriver::waitScanData( + node_info *nodebuffer, + size_t &count, + uint32_t timeout) +{ + if (!m_isConnected) + { + count = 0; + return RESULT_FAIL; + } + + size_t recvNodeCount = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + result_t ans = RESULT_FAIL; + + while ((waitTime = getms() - startTs) < timeout && + recvNodeCount < count) + { + node_info node; + memset(&node, 0, sizeof(node_info)); + ans = waitPackage(&node, timeout - waitTime); + if (!IS_OK(ans)) + { + count = recvNodeCount; + return ans; + } + + nodebuffer[recvNodeCount++] = node; + + if (!nodeIndex) + { + count = recvNodeCount; + return RESULT_OK; + } + + if (recvNodeCount == count) + { + return RESULT_OK; + } + } + + count = recvNodeCount; + return RESULT_FAIL; +} + +result_t GSLidarDriver::grabScanData( + node_info *nodes, + size_t &count, + uint32_t timeout) +{ + uint32_t st = getms(); + uint32_t wt = 0; + while ((wt = getms() - st) < timeout) + { + { + ScopedLocker l(_lock); + if (datas.size()) + { + //从数组中取出点云数据 + gs_module_nodes ns = datas.back(); + datas.clear(); + size_t size = min(int(count), ns.pointCount); + memcpy(nodes, ns.points, size * SDKNODESIZE); + count = size; + return RESULT_OK; + } + } + delay(1); //延时 + } + return RESULT_TIMEOUT; +} + +result_t GSLidarDriver::ascendScanData(node_info *nodebuffer, size_t count) { + float inc_origin_angle = (float)360.0 / count; + int i = 0; + + for (i = 0; i < (int)count; i++) { + if (nodebuffer[i].dist == 0) { + continue; + } else { + while (i != 0) { + i--; + float expect_angle = (nodebuffer[i + 1].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f - inc_origin_angle; + + if (expect_angle < 0.0f) { + expect_angle = 0.0f; + } + + uint16_t checkbit = nodebuffer[i].angle & + LIDAR_RESP_CHECKBIT; + nodebuffer[i].angle = (((uint16_t)(expect_angle * 64.0f)) << + LIDAR_RESP_ANGLE_SHIFT) + checkbit; + } + + break; + } + } + + if (i == (int)count) { + return RESULT_FAIL; + } + + for (i = (int)count - 1; i >= 0; i--) { + if (nodebuffer[i].dist == 0) { + continue; + } else { + while (i != ((int)count - 1)) { + i++; + float expect_angle = (nodebuffer[i - 1].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f + inc_origin_angle; + + if (expect_angle > 360.0f) { + expect_angle -= 360.0f; + } + + uint16_t checkbit = nodebuffer[i].angle & + LIDAR_RESP_CHECKBIT; + nodebuffer[i].angle = (((uint16_t)(expect_angle * 64.0f)) << + LIDAR_RESP_ANGLE_SHIFT) + checkbit; + } + + break; + } + } + + float frontAngle = (nodebuffer[0].angle >> + LIDAR_RESP_ANGLE_SHIFT) / 64.0f; + + for (i = 1; i < (int)count; i++) { + if (nodebuffer[i].dist == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + + if (expect_angle > 360.0f) { + expect_angle -= 360.0f; + } + + uint16_t checkbit = nodebuffer[i].angle & + LIDAR_RESP_CHECKBIT; + nodebuffer[i].angle = (((uint16_t)(expect_angle * 64.0f)) << + LIDAR_RESP_ANGLE_SHIFT) + checkbit; + } + } + + size_t zero_pos = 0; + float pre_degree = (nodebuffer[0].angle >> + LIDAR_RESP_ANGLE_SHIFT) / 64.0f; + + for (i = 1; i < (int)count ; ++i) { + float degree = (nodebuffer[i].angle >> + LIDAR_RESP_ANGLE_SHIFT) / 64.0f; + + if (zero_pos == 0 && (pre_degree - degree > 180)) { + zero_pos = i; + break; + } + + pre_degree = degree; + } + + node_info *tmpbuffer = new node_info[count]; + + for (i = (int)zero_pos; i < (int)count; i++) { + tmpbuffer[i - zero_pos] = nodebuffer[i]; + } + + for (i = 0; i < (int)zero_pos; i++) { + tmpbuffer[i + (int)count - zero_pos] = nodebuffer[i]; + } + + memcpy(nodebuffer, tmpbuffer, count * sizeof(node_info)); + delete[] tmpbuffer; + + return RESULT_OK; +} + +/************************************************************************/ +/* get device parameters of gs lidar */ +/************************************************************************/ +result_t GSLidarDriver::getDevicePara(gs_device_para &info, uint32_t timeout) { + result_t ans; + uint8_t crcSum, mdNum; + uint8_t *pInfo = reinterpret_cast(&info); + + if (!m_isConnected) { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + if ((ans = sendCommand(GS_LIDAR_CMD_GET_PARAMETER)) != RESULT_OK) { + return ans; + } + gs_package_head h; + for (int i = 0; i < LIDAR_MAXCOUNT && i < moduleCount; i++) + { + if ((ans = waitResponseHeaderEx(&h, GS_LIDAR_CMD_GET_PARAMETER, timeout)) != RESULT_OK) { + return ans; + } + if (h.size < (sizeof(gs_device_para) - 1)) { + return RESULT_FAIL; + } + if (waitForData(h.size+1, timeout) != RESULT_OK) { + return RESULT_FAIL; + } + getData(reinterpret_cast(&info), sizeof(info)); + + crcSum = 0; + crcSum += h.address; + crcSum += h.type; + crcSum += 0xff & h.size; + crcSum += 0xff & (h.size >> 8); + for(int j = 0; j < h.size; j++) { + crcSum += pInfo[j]; + } + if(crcSum != info.crc) { + return RESULT_FAIL; + } + + mdNum = h.address >> 1; // 1,2,4 + if (mdNum > 2) { + return RESULT_FAIL; + } + k0[mdNum] = info.k0 / 10000.00; + k1[mdNum] = info.k1 / 10000.00; + b0[mdNum] = info.b0 / 10000.00; + b1[mdNum] = info.b1 / 10000.00; + bias[mdNum] = double(info.bias) * 0.1; + + // debug("k0 %lf k1 %lf b0 %lf b1 %lf bias %lf", + // k0[mdNum], k1[mdNum], b0[mdNum], b1[mdNum], bias[mdNum]); + delay(5); + } + } + + return RESULT_OK; +} + +result_t GSLidarDriver::setDeviceAddress(uint32_t timeout) +{ + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + if (m_SingleChannel) { + return RESULT_OK; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + if ((ans = sendCommand(GS_LIDAR_CMD_GET_ADDRESS)) != RESULT_OK) { + return ans; + } + gs_package_head h; + if ((ans = waitResponseHeaderEx(&h, GS_LIDAR_CMD_GET_ADDRESS, timeout)) != RESULT_OK) { + return ans; + } + moduleCount = (h.address >> 1) + 1; + info("GS lidar count %u", moduleCount); + } + + return RESULT_OK; +} + +/************************************************************************/ +/* the set to signal quality */ +/************************************************************************/ +void GSLidarDriver::setIntensities(const bool &isintensities) +{ + if (m_intensities != isintensities) { + if (globalRecvBuffer) { + delete[] globalRecvBuffer; + globalRecvBuffer = NULL; + } + + globalRecvBuffer = new uint8_t[GSPACKSIZE]; + } + + m_intensities = isintensities; + + if (m_intensities) { + PackageSampleBytes = 2; + } else { + PackageSampleBytes = 2; + } +} +/** +* @brief 设置雷达异常自动重新连接 \n +* @param[in] enable 是否开启自动重连: +* true 开启 +* false 关闭 +*/ +void GSLidarDriver::setAutoReconnect(const bool &enable) +{ + isAutoReconnect = enable; +} + +void GSLidarDriver::checkTransDelay() +{ + //采样率 + trans_delay = _comm->getByteTime(); + sample_rate = 27 * 160; + m_PointTime = 1e9 / sample_rate; +} + +/************************************************************************/ +/* start to scan */ +/************************************************************************/ +result_t GSLidarDriver::startScan(bool force, uint32_t timeout) +{ + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + if (m_isScanning) { + return RESULT_OK; + } + + stop(); + checkTransDelay(); + flushSerial(); + + //获取GS2参数 + gs_device_para gs2_info; + ans = getDevicePara(gs2_info, 300); + if (IS_OK(ans)) + { + flushSerial(); + + ScopedLocker l(_cmd_lock); + if ((ans = sendCommand(GS_LIDAR_CMD_SCAN)) != + RESULT_OK) { + return ans; + } + gs_package_head h; + if ((ans = waitResponseHeaderEx(&h, GS_LIDAR_CMD_SCAN, timeout)) != RESULT_OK) { + return ans; + } + //启动线程 + ans = createThread(); + m_isScanning = true; + } + + return ans; +} + +result_t GSLidarDriver::stopScan(uint32_t timeout) +{ + UNUSED(timeout); + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + ScopedLocker l(_cmd_lock); + if ((ans = sendCommand(GS_LIDAR_CMD_STOP)) != RESULT_OK) { + return ans; + } + gs_package_head h; + if ((ans = waitResponseHeaderEx(&h, GS_LIDAR_CMD_STOP, timeout)) != RESULT_OK) { + return ans; + } + delay(10); + + return RESULT_OK; +} + +result_t GSLidarDriver::createThread() +{ + m_thread = new std::thread(&GSLidarDriver::cacheScanData, this); + if (!m_thread) + { + error("Fail to create GS thread"); + return RESULT_FAIL; + } + + info("Create GS thread 0x%X", m_thread->get_id()); + return RESULT_OK; +} + +result_t GSLidarDriver::startAutoScan(bool force, uint32_t timeout) { + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + flushSerial(); + delay(10); + { + ScopedLocker l(_cmd_lock); + if ((ans = sendCommand(GS_LIDAR_CMD_SCAN)) != + RESULT_OK) { + return ans; + } + + if (!m_SingleChannel) { + gs_package_head h; + if ((ans = waitResponseHeaderEx(&h, GS_LIDAR_CMD_SCAN, timeout)) != RESULT_OK) { + return ans; + } + } + } + + return RESULT_OK; +} + +/************************************************************************/ +/* stop scan */ +/************************************************************************/ +result_t GSLidarDriver::stop() +{ + disableDataGrabbing(); + stopScan(); + + return RESULT_OK; +} + +/************************************************************************/ +/* reset device */ +/************************************************************************/ +result_t GSLidarDriver::reset(uint8_t addr, uint32_t timeout) { + UNUSED(timeout); + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(addr, GS_LIDAR_CMD_RESET)) != RESULT_OK) { + return ans; + } + + return RESULT_OK; +} + +std::string GSLidarDriver::getSDKVersion() { + return YDLIDAR_SDK_VERSION_STR; +} + +std::map GSLidarDriver::lidarPortList() { + std::vector lst = list_ports(); + std::map ports; + + for (std::vector::iterator it = lst.begin(); it != lst.end(); it++) { + std::string port = "ydlidar" + (*it).device_id; + ports[port] = (*it).port; + } + + return ports; +} + +const char *GSLidarDriver::DescribeError(bool isTCP) +{ + if (_comm) { + return _comm->DescribeError(); + } + return nullptr; +} + +result_t GSLidarDriver::getHealth(device_health &, uint32_t) +{ + return RESULT_OK; +} + +result_t GSLidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) +{ + result_t ret = RESULT_OK; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + //尝试获取雷达型号 + ret = getDeviceInfo2(info, timeout); + if (!IS_OK(ret)) + { + for (int i=0; i &dis, + uint32_t timeout) +{ + //1、获取级联雷达数量 + result_t ret = setDeviceAddress(timeout); + if (!IS_OK(ret)) + { + error("Fail to get GS lidar count"); + return ret; + } + //2、获取设备信息（带雷达型号码） + uint8_t c = moduleCount; + ScopedLocker l(_lock); + ret = sendCommand(GS_LIDAR_CMD_GET_VERSION3); + if (!IS_OK(ret)) + return ret; + for (uint8_t i=0; i(&gsdi2), GSDEVINFO2SIZE); + + device_info_ex di; + di.id = head.address >> 1; + di.di.model = uint8_t(gsdi2.model); + di.di.hardware_version = gsdi2.hwVersion; + di.di.firmware_version = uint16_t((gsdi2.fwVersion & 0xFF) << 8) + + uint16_t(gsdi2.fwVersion >> 8); + memcpy(di.di.serialnum, gsdi2.sn, SDK_SNLEN); + dis.push_back(di); + } + if (IS_OK(ret)) + return ret; + //3、获取设备信息（不带雷达型号码） + ret = sendCommand(GS_LIDAR_CMD_GET_VERSION); + for (uint8_t i=0; i(&gsdi), sizeof(gsdi)); + + device_info_ex di; + di.id = head.address >> 1; + di.di.model = YDLIDAR_GS2; + di.di.hardware_version = gsdi.hwVersion; + di.di.firmware_version = uint16_t((gsdi.fwVersion & 0xFF) << 8) + + uint16_t(gsdi.fwVersion >> 8); + memcpy(di.di.serialnum, gsdi.sn, SDK_SNLEN); + dis.push_back(di); + } + + return ret; +} + +result_t GSLidarDriver::getDeviceInfo1(device_info &info, uint32_t timeout) +{ + result_t ret = RESULT_FAIL; + + ScopedLocker l(_cmd_lock); + if ((ret = sendCommand(GS_LIDAR_CMD_GET_VERSION)) != RESULT_OK) { + return ret; + } + uint8_t c = std::min(moduleCount, uint8_t(LIDAR_MAXCOUNT)); + for (uint8_t i=0; i(&di), sizeof(di)); + + if (LIDAR_MODULE_1 == head.address) + { + info.hardware_version = di.hwVersion; + info.firmware_version = uint16_t((di.fwVersion & 0xFF) << 8) + + uint16_t(di.fwVersion >> 8); + memcpy(info.serialnum, di.sn, SDK_SNLEN); + // head.address; //雷达序号 + m_HasDeviceInfo |= EPT_Module | EPT_Base; + } + } + + return ret; +} + +result_t GSLidarDriver::getDeviceInfo2(device_info &dev, uint32_t timeout) +{ + result_t ret = RESULT_FAIL; + + //获取设备信息，包含雷达型号（对外协议） + ScopedLocker l(_lock); + ret = sendCommand(GS_LIDAR_CMD_GET_VERSION3); + if (!IS_OK(ret)) + return ret; + uint8_t c = std::min(moduleCount, uint8_t(LIDAR_MAXCOUNT)); + for (uint8_t i=0; i(&di), GSDEVINFO2SIZE); + + uint8_t id = uint8_t(head.address >> 1); //模组地址转编号: 1, 2, 4 + m_models[id] = di.model; + info("Get Module[%d] Lidar model[%u]", id, di.model); + if (LIDAR_MODULE_1 == head.address) + { + dev.model = uint8_t(di.model); + dev.hardware_version = di.hwVersion; + dev.firmware_version = uint16_t((di.fwVersion & 0xFF) << 8) + + uint16_t(di.fwVersion >> 8); + memcpy(dev.serialnum, di.sn, SDK_SNLEN); + m_HasDeviceInfo |= EPT_Module | EPT_Base; + } + } + + return ret; +} + +result_t GSLidarDriver::setWorkMode(int mode, uint8_t addr) +{ + result_t ans; + uint32_t timeout = 300; + string buf; + + if (!isconnected()) { + return RESULT_FAIL; + } + + //如果已经开启扫描，则先停止扫描 + if (isscanning()) + { + disableDataGrabbing(); + delay(10); + stopScan(); + } + flushSerial(); + + { + ScopedLocker l(_cmd_lock); + uint8_t m = uint8_t(mode); + if ((ans = sendCommand(addr, GS_LIDAR_CMD_SET_MODE, &m, 1)) != RESULT_OK) { + return ans; + } + gs_package_head response_header; + if ((ans = waitResponseHeaderEx(&response_header, GS_LIDAR_CMD_SET_MODE, timeout)) != RESULT_OK) { + return ans; + } + if (response_header.type != GS_LIDAR_CMD_SET_MODE) { + return RESULT_FAIL; + } + } + + return RESULT_OK; +} + +bool GSLidarDriver::ota() +{ + if (m_OtaName.empty()) + { + error("[OTA] Not set OTA file"); + return false; + } + // 读取文件所有内容 + std::ifstream f; + f.open(m_OtaName, ios::in | ios::binary); + if (!f.is_open()) + { + error("[OTA] Fail to open OTA file[%s]", m_OtaName.c_str()); + return false; + } + //读数据 + std::vector data; + while (!f.eof()) + { + std::vector d(DATA_LEN_PER_FRAME); + memset(d.data(), GS_CMD_ZERO, d.size()); + f.read(reinterpret_cast(d.data()), d.size()); + int s = f.gcount(); //获取读取成功的字节数 + for (int i=0; i d; + uint8_t dsr[] = {0x00, 0x00, + 0x73, 0x74, 0x61, 0x72, 0x74, 0x20, 0x64, 0x6F, + 0x77, 0x6E, 0x6C, 0x6F, 0x61, 0x64, 0x00, 0x00}; + for (int i = 0; i < sizeof(dsr); ++i) + d.push_back(dsr[i]); + std::vector dataRecv; + bool ret = sendData( + addr, + GS_CMD_STARTIAP, + d, + GS_CMD_ACKIAP, + dataRecv, + TIMEOUT_500); + if (ret) + { + ret = isOtaRespOk( + addr, + GS_CMD_STARTIAP, + GS_CMD_ZERO, + dataRecv); + } + + return ret; +} + +bool GSLidarDriver::execOta(uint8_t addr, const std::vector& data) +{ + // 数据中固定部分（字符串“downloading”） + uint8_t fix[] = {0x64, 0x6F, 0x77, 0x6E, 0x6C, 0x6F, 0x61, 0x64, + 0x69, 0x6E, 0x67, 0x00, 0x00, 0x00, 0x00, 0x00}; + bool ret = false; + // 计算固件分成的数据包数 + int n = data.size() % DATA_LEN_PER_FRAME; + int m = data.size() / DATA_LEN_PER_FRAME + (n ? 1 : 0); + + int percent = -1; + for (int j = 0; j < m; ++j) + { + // 打印进度 + int p = int(j * 100.0 / m); + if (p != percent) + { + percent = p; + info("[OTA] Downloading [%d%%]", p); + } + + std::vector d; + int offset = j * DATA_LEN_PER_FRAME; // 数据偏移量 + d.push_back(offset & 0xFF); + d.push_back(offset >> 8); + for (int i = 0; i < sizeof(fix); ++i) + d.push_back(fix[i]); + for (int i = offset; i < offset + DATA_LEN_PER_FRAME; ++i) + { + if (i < data.size()) + d.push_back(data.at(i)); + else + d.push_back(GS_CMD_ZERO); + } + + std::vector dataRecv; + ret = sendData( + addr, + GS_CMD_EXECIAP, + d, + GS_CMD_ACKIAP, + dataRecv, + TIMEOUT_500); + if (ret) + { + ret = isOtaRespOk( + addr, + GS_CMD_EXECIAP, + uint16_t(offset), + dataRecv); + } + if (!ret) + { + error("[OTA] Fail to download [%d] package", j + 1); + break; + } + } + + return ret; +} + +bool GSLidarDriver::stopOta(uint8_t addr) +{ + std::vector d; + + uint8_t dsr[] = {0x00, 0x00, + 0x63, 0x6F, 0x6D, 0x70, 0x6C, 0x65, 0x74, 0x65, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + for (int i = 0; i < sizeof(dsr); ++i) + d.push_back(dsr[i]); + // 是否加密标识 + d.push_back(m_OtaEncode); + d.push_back(GS_CMD_ZERO); + d.push_back(GS_CMD_ZERO); + d.push_back(GS_CMD_ZERO); + + std::vector dataRecv; + bool ret = sendData( + addr, + GS_CMD_STOPIAP, + d, + GS_CMD_ACKIAP, + dataRecv, + TIMEOUT_500); + if (ret) + { + ret = isOtaRespOk( + addr, + GS_CMD_STOPIAP, + GS_CMD_ZERO, + dataRecv); + } + + return ret; +} + +bool GSLidarDriver::isOtaRespOk( + uint8_t addr, + uint8_t cmd, + uint16_t offset, + const std::vector &data) +{ + std::vector d; + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(addr); + d.push_back(GS_CMD_ACKIAP); + uint16_t len = 4; + d.push_back(len & 0xFF); + d.push_back(len >> 8); + d.push_back(offset & 0xFF); + d.push_back(offset >> 8); + d.push_back(cmd); + d.push_back(GS_CMD_ACKOK); + //计算8位校验和 + uint8_t cs = 0; + for (int k=4; k &data, + uint8_t cmdRecv, + std::vector &dataRecv, + int timeout) +{ + std::vector d; + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(LIDAR_CMD_SYNC_BYTE); + d.push_back(addr); + d.push_back(cmd); + uint16_t len = uint16_t(data.size()); + d.push_back(len & 0xFF); + d.push_back(len >> 8); + for (size_t i=0; i ds; + //计算8位校验和 + uint8_t cs = 0; + for (int k=4; k dRecv(GSPACKEGEHEADSIZE + head.size + 1); + memcpy(&dRecv[0], &head, GSPACKEGEHEADSIZE); + r = getData(&dRecv[GSPACKEGEHEADSIZE], head.size + 1); + if (IS_OK(r)) + { + dataRecv = dRecv; + ret = true; + } + + return ret; +} + +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.h new file mode 100644 index 0000000..875c69b --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/GSLidarDriver.h @@ -0,0 +1,520 @@ +/********************************************************************* + * 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. + *********************************************************************/ + +/** @page GSLidarDriver + * GSLidarDriver API + +
Library GSLidarDriver +
File GSLidarDriver.h +
Author Tony [code at ydlidar com] +
Source https://github.com/ydlidar/YDLidar-SDK +
Version 1.0.0 +
+ This GSLidarDriver support [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) and [TYPE_TOF](\ref LidarTypeID::TYPE_TOF) LiDAR + +* @copyright Copyright (c) 2018-2020 EAIBOT + Jump to the @link ::ydlidar::GSLidarDriver @endlink interface documentation. +*/ +#ifndef GS2_YDLIDAR_DRIVER_H +#define GS2_YDLIDAR_DRIVER_H + +#include +#include +#include +#include +#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 + +using namespace std; + +namespace ydlidar +{ + + using namespace core; + using namespace core::serial; + using namespace core::base; + + /*! + * GS雷达操控类 + */ + class GSLidarDriver : public DriverInterface + { + public: + /*! + * A constructor. + * A more elaborate description of the constructor. + */ + GSLidarDriver(uint8_t type = YDLIDAR_TYPE_SERIAL); + /*! + * A destructor. + * A more elaborate description of the destructor. + */ + virtual ~GSLidarDriver(); + + /*! + * @brief 连接雷达 \n + * 连接成功后，必须使用::disconnect函数关闭 + * @param[in] port_path 串口号 + * @param[in] baudrate 波特率，YDLIDAR-GS2 雷达波特率：961200 + * @return 返回连接状态 + * @retval 0 成功 + * @retval < 0 失败 + * @note连接成功后，必须使用::disconnect函数关闭 + * @see 函数::GSLidarDriver::disconnect (“::”是指定有连接功能,可以看文档里的disconnect变成绿,点击它可以跳转到disconnect.) + */ + result_t connect(const char *port_path, uint32_t baudrate); + + /*! + * @brief 断开雷达连接 + */ + void disconnect(); + + /*! + * @brief 获取当前SDK版本号 \n + * 静态函数 + * @return 返回当前SKD 版本号 + */ + virtual std::string getSDKVersion(); + + /*! + * @brief lidarPortList 获取雷达端口 + * @return 在线雷达列表 + */ + static std::map lidarPortList(); + + /*! + * @brief 扫图状态 \n + * @return 返回当前雷达扫图状态 + * @retval true 正在扫图 + * @retval false 扫图关闭 + */ + bool isscanning() const; + + /*! + * @brief 连接雷达状态 \n + * @return 返回连接状态 + * @retval true 成功 + * @retval false 失败 + */ + bool isconnected() const; + + /*! + * @brief 设置雷达是否带信号质量 \n + * 连接成功后，必须使用::disconnect函数关闭 + * @param[in] isintensities 是否带信号质量: + * true 带信号质量 + * false 无信号质量 + * @note只有S4B(波特率是153600)雷达支持带信号质量, 别的型号雷达暂不支持 + */ + void setIntensities(const bool &isintensities); + + /*! + * @brief 设置雷达异常自动重新连接 \n + * @param[in] enable 是否开启自动重连: + * true 开启 + * false 关闭 + */ + void setAutoReconnect(const bool &enable); + + /*! + * @brief 获取雷达设备信息 \n + * @param[in] parameters 设备信息 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE or RESULT_TIMEOUT 获取失败 + */ + result_t getDevicePara(gs_device_para &info, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 配置雷达地址 \n + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 配置成功 + * @retval RESULT_FAILE or RESULT_TIMEOUT 配置超时 + */ + result_t setDeviceAddress(uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 开启扫描 \n + * @param[in] force 扫描模式 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 开启成功 + * @retval RESULT_FAILE 开启失败 + * @note 只用开启一次成功即可 + */ + result_t startScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 关闭扫描 \n + * @return 返回执行结果 + * @retval RESULT_OK 关闭成功 + * @retval RESULT_FAILE 关闭失败 + */ + result_t stop(); + + /*! + * @brief 获取激光数据 \n + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + * @note 获取之前，必须使用::startScan函数开启扫描 + */ + result_t grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 补偿激光角度 \n + * 把角度限制在0到360度之间 + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 补偿之前，必须使用::grabScanData函数获取激光数据成功 + */ + result_t ascendScanData(node_info *nodebuffer, size_t count); + + /*! + * @brief 重置激光雷达 \n + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作, 如果在扫描中调用::stop函数停止扫描 + */ + result_t reset(uint8_t addr, uint32_t timeout = DEFAULT_TIMEOUT); + + protected: + /*! + * @brief 创建解析雷达数据线程 \n + * @note 创建解析雷达数据线程之前，必须使用::startScan函数开启扫图成功 + */ + result_t createThread(); + + /*! + * @brief 重新连接开启扫描 \n + * @param[in] force 扫描模式 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 开启成功 + * @retval RESULT_FAILE 开启失败 + * @note sdk 自动重新连接调用 + */ + result_t startAutoScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief stopScan + * @param timeout + * @return + */ + result_t stopScan(uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief waitDevicePackage + * @param timeout + * @return + */ + result_t waitDevicePackage(uint32_t timeout = DEFAULT_TIMEOUT); + /*! + * @brief 解包激光数据 \n + * @param[in] node 解包后激光点信息 + * @param[in] timeout 超时时间 + */ + result_t waitPackage(node_info *node, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] nodebuffer 激光信息指针 + * @param[in] count 激光点数大小 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 失败 + */ + result_t waitScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 激光数据解析线程 \n + */ + int cacheScanData(); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] cmd 命名码 + * @param[in] payload payload + * @param[in] payloadsize payloadsize + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + */ + result_t sendCommand(uint8_t cmd, + const void *payload = NULL, + size_t payloadsize = 0); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] addr 模组地址 + * @param[in] cmd 命名码 + * @param[in] payload payload + * @param[in] payloadsize payloadsize + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + */ + result_t sendCommand(uint8_t addr, + uint8_t cmd, + const void *payload = NULL, + size_t payloadsize = 0); + + /*! + * @brief 等待激光数据包头 \n + * @param[in] header 包头 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 获取失败 + * @note 当timeout = -1 时, 将一直等待 + */ + result_t waitResponseHeader(gs_package_head *header, + uint32_t timeout = DEFAULT_TIMEOUT); + result_t waitResponseHeaderEx(gs_package_head *header, + uint8_t cmd, + uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 等待固定数量串口数据 \n + * @param[in] data_count 等待数据大小 + * @param[in] timeout 等待时间 + * @param[in] returned_size 实际数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 获取失败 + * @note 当timeout = -1 时, 将一直等待 + */ + result_t waitForData(size_t data_count, uint32_t timeout = DEFAULT_TIMEOUT, + size_t *returned_size = NULL); + + /*! + * @brief 获取串口数据 \n + * @param[in] data 数据指针 + * @param[in] size 数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + */ + result_t getData(uint8_t *data, size_t size); + + /*! + * @brief 串口发送数据 \n + * @param[in] data 发送数据指针 + * @param[in] size 数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 发送成功 + * @retval RESULT_FAILE 发送失败 + */ + result_t sendData(const uint8_t *data, size_t size); + + /*! + * @brief checkTransDelay + */ + void checkTransDelay(); + + /*! + * @brief 关闭数据获取通道 \n + */ + void disableDataGrabbing(); + + /*! + * @brief 设置串口DTR \n + */ + void setDTR(); + + /*! + * @brief 清除串口DTR \n + */ + void clearDTR(); + + /*! + * @brief flushSerial + */ + void flushSerial(); + + /*! + * @brief checkAutoConnecting + */ + result_t checkAutoConnecting(); + + /*! + * @brief 换算得出点的距离和角度 + */ + void angTransform(uint16_t dist, int n, double *dstTheta, uint16_t *dstDist); + void angTransform2(uint16_t dist, int n, double *dstTheta, uint16_t *dstDist); + + /** + * @brief 串口错误信息 + * @param isTCP TCP or UDP + * @return error information + */ + virtual const char *DescribeError(bool isTCP = false); + + /** + * @brief GS2雷达没有健康信息\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); + + //获取设备信息 + virtual result_t getDeviceInfo( + device_info &di, + uint32_t timeout = DEFAULT_TIMEOUT/4); + //获取级联雷达设备信息 + virtual result_t getDeviceInfo( + std::vector &dis, + uint32_t timeout = DEFAULT_TIMEOUT); + virtual result_t getDeviceInfo1( + device_info &di, + uint32_t timeout = DEFAULT_TIMEOUT); + virtual result_t getDeviceInfo2( + device_info &di, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief 设置雷达工作模式（目前只针对GS2雷达） + * @param[in] mode 雷达工作模式，0为避障模式；1为延边模式 + * @param[in] addr 雷达地址，第1个雷达地址为0x01；第2个雷达地址为0x02；第3个雷达地址为0x04； + * @return 成功返回RESULT_OK，否则返回非RESULT_OK + */ + virtual result_t setWorkMode(int mode = 0, uint8_t addr = 0x00); + + // 开始OTA升级 + virtual bool ota(); + // 开始OTA + bool startOta(uint8_t addr); + // OTA升级中 + bool execOta(uint8_t addr, const std::vector& data); + // 停止OTA + bool stopOta(uint8_t addr); + //判断响应是否正常 + bool isOtaRespOk(uint8_t addr, + uint8_t cmd, + uint16_t offset, + const std::vector& data); + bool sendData(uint8_t addr, + uint8_t cmd, + const std::vector &data, + uint8_t cmdRecv, + std::vector &dataRecv, + int timeout = 500); + + // 未实现的虚函数 + virtual result_t getScanFrequency(scan_frequency &frequency, uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyDis(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyAdd(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyAddMic(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyDisMic(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t getSamplingRate(sampling_rate &rate, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setSamplingRate(sampling_rate &rate, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t getZeroOffsetAngle(offset_angle &angle, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanHeartbeat(scan_heart_beat &beat, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + + public: + enum + { + DEFAULT_TIMEOUT = 2000, /**< 默认超时时间. */ + DEFAULT_HEART_BEAT = 1000, /**< 默认检测掉电功能时间. */ + MAX_SCAN_NODES = 160 * 3, /**< 最大扫描点数. */ + DEFAULT_TIMEOUT_COUNT = 3, // 错误数 + }; + + private: + int PackageSampleBytes; //一个包包含的激光点数 + ChannelDevice *_comm = nullptr; //通讯对象 + uint32_t trans_delay; //串口传输一个byte时间 + int sample_rate; //采样频率 + + gs_node_package package; //带信号质量协议包 + + uint8_t CheckSum; //校验和 + uint8_t CheckSumCal; + bool CheckSumResult; + + uint8_t *globalRecvBuffer = nullptr; + + double k0[LIDAR_MAXCOUNT]; + double k1[LIDAR_MAXCOUNT]; + double b0[LIDAR_MAXCOUNT]; + double b1[LIDAR_MAXCOUNT]; + double bias[LIDAR_MAXCOUNT]; + int m_models[LIDAR_MAXCOUNT] = {0}; + int model = YDLIDAR_GS2; //雷达型号 + uint8_t moduleNum = 0; // 模块编号 + uint8_t moduleCount = 1; // 当前模组数量 + int nodeCount = 0; //当前包点数 + uint64_t stamp = 0; //时间戳 + std::list datas; //各模组数据 + double m_pitchAngle = Angle_PAngle; + uint32_t lastStamp = 0; //上一次时间 + }; + +} // namespace ydlidar + +#endif // GS2_YDLIDAR_DRIVER_H diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.cpp new file mode 100644 index 0000000..5037dc9 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.cpp @@ -0,0 +1,999 @@ +/********************************************************************* + * 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. + *********************************************************************/ +#include +#include +#include "SDMLidarDriver.h" +#include "core/serial/common.h" +#include "ydlidar_config.h" + +using namespace impl; + +namespace ydlidar +{ + +SDMLidarDriver::SDMLidarDriver() + : _serial(NULL) +{ + // 串口配置参数 + m_intensities = false; + isAutoReconnect = true; + isAutoconnting = false; + m_baudrate = 230400; + m_PointTime = 1e9 / 5000; + retryCount = 0; + m_SingleChannel = false; + m_LidarType = TYPE_SDM; + + nodeIndex = 0; + recvBuff = std::vector(SDKSDMPCSSIZE, 0); + + scan_node_count = 0; + scan_node_buf = new node_info[SDK_SDM_POINT_COUNT * 5]; +} + +SDMLidarDriver::~SDMLidarDriver() +{ + m_isScanning = false; + isAutoReconnect = false; + _thread.join(); + + ScopedLocker l(_cmd_lock); + if (_serial) + { + if (_serial->isOpen()) + { + _serial->flush(); + _serial->closePort(); + } + } + if (_serial) + { + delete _serial; + _serial = NULL; + } + + if (scan_node_buf) + { + delete[] scan_node_buf; + scan_node_buf = NULL; + } +} + +result_t SDMLidarDriver::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 SDMLidarDriver::setDTR() +{ + if (!m_isConnected) + return; + + if (_serial) + _serial->setDTR(1); +} + +void SDMLidarDriver::clearDTR() +{ + if (!m_isConnected) + return; + + if (_serial) + _serial->setDTR(0); +} + +void SDMLidarDriver::flushSerial() +{ + if (!m_isConnected) + { + return; + } + + size_t len = _serial->available(); + if (len) + { + _serial->readSize(len); + } + + delay(20); +} + +void SDMLidarDriver::disconnect() +{ + isAutoReconnect = false; + + if (!m_isConnected) + { + return; + } + + stop(); + delay(10); + ScopedLocker l(_cmd_lock); + + if (_serial) + { + if (_serial->isOpen()) + { + _serial->closePort(); + } + } + + m_isConnected = false; +} + +void SDMLidarDriver::disableDataGrabbing() +{ + if (m_isScanning) + { + m_isScanning = false; + _dataEvent.set(); + } + _thread.join(); +} + +bool SDMLidarDriver::isscanning() const +{ + return m_isScanning; +} +bool SDMLidarDriver::isconnected() const +{ + return m_isConnected; +} + +result_t SDMLidarDriver::sendCmd( + uint8_t cmd, + const uint8_t *data, + size_t dataSize) +{ + if (!m_isConnected) + return RESULT_FAIL; + + size_t size = SDKSDMHEADSIZE + dataSize; + std::vector buff(size + 1, 0); + + SdkSdmHead head; + head.head0 = SDK_CMD_HEADFLAG0; + head.head1 = SDK_CMD_HEADFLAG1; + head.cmd = cmd; + head.size = uint8_t(dataSize); + memcpy(&buff[0], &head, SDKSDMHEADSIZE); + + if (data && dataSize) + memcpy(&buff[SDKSDMHEADSIZE], data, dataSize); + + uint8_t cs = 0; + for (size_t i = 0; i < size; ++i) + cs += buff[i]; + buff[size] = cs; + + return sendData(buff.data(), buff.size()); +} + +result_t SDMLidarDriver::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 SDMLidarDriver::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 SDMLidarDriver::waitResp( + uint8_t cmd, + uint32_t timeout) +{ + std::vector data; + return waitResp(cmd, data, timeout); +} + +result_t SDMLidarDriver::waitResp( + uint8_t cmd, + std::vector &data, + uint32_t timeout) +{ + int pos = 0; + uint32_t st = getms(); + uint32_t wt = 0; + std::vector recvBuff(SDK_BUFFER_MAXLEN, 0); + SdkSdmHead head; + uint8_t *buff = reinterpret_cast(&head); + uint8_t cs = 0; + uint8_t dataSize = 0; + + while ((wt = getms() - st) <= timeout) + { + size_t srcSize = SDKSDMHEADSIZE - pos; + size_t dstSize = 0; + + 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_HEADFLAG0) + { + pos = 0; + continue; + } + break; + case 1: + if (c != SDK_CMD_HEADFLAG1) + { + pos = 0; + continue; + } + break; + case 2: + if (c != cmd) //判断解析到的命令字是否和指定命令字是否一致 + { + pos = 0; + cs = 0; + continue; + } + break; + case 3: + dataSize = c; + break; + default: + break; + } + + buff[pos++] = c; + cs += c; + } + + //如果找到协议头 + if (pos == SDKSDMHEADSIZE) + { + //获取剩余数据，并计算校验和 + size_t srcSize = dataSize + 1; + size_t dstSize = 0; + 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) + { + cs += recvBuff[i]; + data.push_back(recvBuff[i]); //数据存入输出参数 + } + if (cs != recvBuff[dataSize]) //判断校验和是否一致 + { + printf("[YDLIDAR] CMD CheckSum error calc[0x%02X] != src[0x%02X]\n", + cs, recvBuff[dataSize]); + return RESULT_FAIL; + } + return RESULT_OK; + } + } + + return RESULT_FAIL; +} + +result_t SDMLidarDriver::waitForData( + size_t srcSize, + uint32_t timeout, + size_t *dstSize) +{ + size_t size = 0; + if (!dstSize) + dstSize = &size; + + result_t ret = _serial->waitfordata(srcSize, timeout, dstSize); + if (IS_OK(ret)) + { + if (*dstSize > srcSize) + *dstSize = srcSize; + } + return ret; +} + +result_t SDMLidarDriver::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 = NULL; + } + } + } + retryCount ++; + if (retryCount > 10) + { + retryCount = 10; + } + + delay(100 * retryCount); + int retryConnect = 0; + + while (isAutoReconnect && + connect(m_port.c_str(), m_baudrate) != RESULT_OK) + { + retryConnect++; + if (retryConnect > 10) + { + retryConnect = 10; + } + + delay(100); + } + + if (!isAutoReconnect) + { + m_isScanning = false; + return RESULT_FAIL; + } + + if (isconnected()) + { + delay(100); + { + ans = startAutoScan(); + if (!IS_OK(ans)) + { + ans = startAutoScan(); + } + } + + if (IS_OK(ans)) + { + isAutoconnting = false; + return ans; + } + } + } + + return RESULT_FAIL; +} + +int SDMLidarDriver::cacheScanData() +{ + node_info local_buf[SDK_SDM_POINT_COUNT]; + size_t count = SDK_SDM_POINT_COUNT; + size_t scan_count = 0; + result_t ret = RESULT_FAIL; + + int timeout_count = 0; + retryCount = 0; + + m_isScanning = true; + + while (m_isScanning) + { + count = SDK_SDM_POINT_COUNT; + ret = waitScanData(local_buf, count); + if (!IS_OK(ret)) // 如果解析点云失败 + { + if (IS_FAIL(ret) || + 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); + } + continue; + } + else + { + timeout_count = 0; + retryCount = 0; + + // printf("[YDLIDAR] SDM points Stored in buffer %lu\n", count); + ScopedLocker l(_lock); + memcpy(scan_node_buf, local_buf, sizeof(node_info) * SDK_SDM_POINT_COUNT); + scan_node_count = SDK_SDM_POINT_COUNT; // 一个包固定1个点 + _dataEvent.set(); + scan_count = 0; + } + } + + m_isScanning = false; + + return RESULT_OK; +} + +result_t SDMLidarDriver::waitPackage(node_info *node, uint32_t timeout) +{ + int pos = 0; + uint32_t st = getms(); + uint32_t wt = 0; + uint8_t dataSize = 0; + uint8_t cs = 0; + result_t ret = RESULT_FAIL; + SdkSdmPcs pcs; + uint8_t *buff = reinterpret_cast(&pcs); + + memset(node, 0, SDKNODESIZE); + + while ((wt = getms() - st) <= timeout) + { + size_t srcSize = SDKSDMHEADSIZE - pos; + size_t dstSize = 0; + + 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_HEADFLAG0) + { + pos = 0; + continue; + } + break; + case 1: + if (c != SDK_CMD_HEADFLAG1) + { + pos = 0; + continue; + } + break; + case 2: + if (c != SDK_CMD_STARTSCAN) + { + pos = 0; + cs = 0; + continue; + } + break; + case 3: + dataSize = c; + if (!c) // 如果数据长度无效则跳过 + { + pos = 0; + cs = 0; + continue; + } + break; + default: + break; + } + + buff[pos++] = c; + cs += c; + } + + // 如果找到协议头 + if (pos == SDKSDMHEADSIZE) + { + // 获取剩余数据，并计算校验和 + size_t srcSize = dataSize + 1; + size_t dstSize = 0; + ret = waitForData(srcSize, timeout - wt, &dstSize); + if (!IS_OK(ret)) + return ret; + getData(recvBuff.data(), dstSize); + for (size_t i = 0; i < dataSize; ++i) + { + cs += recvBuff[i]; + } + if (cs != recvBuff[dataSize]) + { + printf("[YDLIDAR] PC CheckSum error calc[0x%02X] != src[0x%02X]\n", + cs, recvBuff[dataSize]); + return RESULT_FAIL; + } + memcpy(&buff[SDKSDMHEADSIZE], recvBuff.data(), dstSize); + break; + } + } + + if (IS_OK(ret)) + { + (*node).sync = NODE_SYNC; + (*node).stamp = getTime(); + (*node).index = 0; + (*node).scanFreq = m_ScanFreq; + (*node).qual = 0; + + (*node).dist = pcs.point.dist; + (*node).angle = 0; + (*node).qual = pcs.point.intensity; + (*node).is = pcs.point.env; + return RESULT_OK; + } + else + { + return RESULT_FAIL; + } +} + +result_t SDMLidarDriver::waitScanData( + node_info *nodes, + size_t &count, + uint32_t timeout) +{ + result_t ret = RESULT_FAIL; + if (!m_isConnected) + { + count = 0; + return ret; + } + + 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; +} + +result_t SDMLidarDriver::grabScanData( + node_info *nodebuffer, + size_t &count, + uint32_t timeout) +{ + switch (_dataEvent.wait(timeout)) + { + case Event::EVENT_TIMEOUT: + count = 0; + return RESULT_TIMEOUT; + case Event::EVENT_OK: + { + ScopedLocker l(_lock); + size_t size_to_copy = min(count, scan_node_count); + memcpy(nodebuffer, scan_node_buf, size_to_copy * SDKNODESIZE); + count = size_to_copy; + scan_node_count = 0; + return RESULT_OK; + } + default: + count = 0; + return RESULT_FAIL; + } +} + +/** + * @brief 设置雷达异常自动重新连接 \n + * @param[in] enable 是否开启自动重连: + * true 开启 + * false 关闭 + */ +void SDMLidarDriver::setAutoReconnect(const bool &enable) +{ + isAutoReconnect = enable; +} + +// void SDMLidarDriver::checkTransDelay() +// { +// //采样率 +// trans_delay = _serial->getByteTime(); +// sample_rate = 27 * 160; +// m_PointTime = 1e9 / sample_rate; +// } + +/************************************************************************/ +/* start to scan */ +/************************************************************************/ +result_t SDMLidarDriver::startScan(bool force, uint32_t timeout) +{ + result_t ret = RESULT_FAIL; + + if (!m_isConnected) + return RESULT_FAIL; + if (m_isScanning) + return RESULT_OK; + + stopScan(); //启动前先停止 + //启动前先设置扫描频率 + if (!IS_OK(setScanFreq(m_ScanFreq, 500))) + { + printf("[YDLIDAR] Set scan frequency %.01f failed!\n", m_ScanFreq); + return RESULT_FAIL; + } + + { + //发送启动雷达命令 + ScopedLocker l(_cmd_lock); + if ((ret = sendCmd(SDK_CMD_STARTSCAN)) != RESULT_OK) + return ret; + //双通雷达才发送启动命令 + if (!m_SingleChannel) + { + ret = waitResp(SDK_CMD_STARTSCAN, timeout); + if (!IS_OK(ret)) + { + printf("[YDLIDAR] Response to start scan error!\n"); + return ret; + } + } + + ret = createThread(); + } + + return ret; +} + +result_t SDMLidarDriver::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 SDMLidarDriver::createThread() +{ + // 如果线程已启动，则先退出线程 + if (_thread.getHandle()) + { + m_isScanning = false; + _thread.join(); + } + _thread = CLASS_THREAD(SDMLidarDriver, cacheScanData); + if (!_thread.getHandle()) + { + printf("[YDLIDAR] Fail to create SDM thread\n"); + return RESULT_FAIL; + } + + printf("[YDLIDAR] Create SDM thread 0x%X\n", _thread.getHandle()); + fflush(stdout); + + return RESULT_OK; +} + +result_t SDMLidarDriver::startAutoScan(bool force, uint32_t timeout) +{ + result_t ans; + + if (!m_isConnected) + return RESULT_FAIL; + + flushSerial(); + delay(10); + { + ScopedLocker l(_cmd_lock); + if ((ans = sendCmd(SDK_CMD_STARTSCAN)) != + RESULT_OK) + { + return ans; + } + if (!m_SingleChannel) + { + if ((ans = waitResp(SDK_CMD_STARTSCAN, timeout)) != RESULT_OK) + { + return ans; + } + } + } + + return RESULT_OK; +} + +/************************************************************************/ +/* stop scan */ +/************************************************************************/ +result_t SDMLidarDriver::stop() +{ + if (isAutoconnting) + isAutoReconnect = false; + + disableDataGrabbing(); + stopScan(); + flushSerial(); + + return RESULT_OK; +} + +/************************************************************************/ +/* reset device */ +/************************************************************************/ +result_t SDMLidarDriver::reset(uint8_t addr, uint32_t timeout) +{ + UNUSED(timeout); + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + ScopedLocker l(_cmd_lock); + if ((ans = sendCmd(SDK_CMD_RESET)) != RESULT_OK) + { + return ans; + } + + return RESULT_OK; +} + +result_t SDMLidarDriver::enableFilter(bool yes) +{ + result_t ret = RESULT_FAIL; + if (!m_isConnected) + return ret; + + ScopedLocker l(_cmd_lock); + uint8_t e = yes ? 0x01 : 0x00; + if ((ret = sendCmd(SDK_CMD_SETFILTER, &e, 1)) != RESULT_OK) + return ret; + + return ret; +} + +result_t SDMLidarDriver::setScanFreq(float sf, uint32_t timeout) +{ + result_t ret = RESULT_FAIL; + if (!m_isConnected) + return ret; + + static float s_sfs[] = {10.01, 100.01, 200.01, 500.01, 1000.01, 1800.01}; + uint8_t d = 0x00; + int size = sizeof(s_sfs) / sizeof(float); + for (int i=0; i data; + ret = waitResp(SDK_CMD_SETFREQ, data, timeout); + if (!IS_OK(ret)) + return ret; + if (!data.size()) + return RESULT_FAIL; + d = data.at(0); + //根据返回的扫描频率更新当前扫描频率 + for (int i=0; iDescribeError(); + } + return nullptr; +} + +result_t SDMLidarDriver::getHealth(device_health &health, uint32_t timeout) +{ + return RESULT_OK; +} + +result_t SDMLidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) +{ + result_t ret = RESULT_OK; + + if (!m_isConnected) + return RESULT_FAIL; + + disableDataGrabbing(); + + ScopedLocker l(_cmd_lock); + if ((ret = sendCmd(SDK_CMD_GETVERSION)) != RESULT_OK) + return ret; + + std::vector data; + if ((ret = waitResp(SDK_CMD_GETVERSION, data, timeout)) != RESULT_OK) + return ret; + + // printf("%s %llu\n", __FUNCTION__, data.size()); + + if (data.size() == SDKSDMDEVICEINFOSIZE) //新版设备信息（带序列号） + { + SdkSdmDeviceInfo di; + memcpy(&di, data.data(), data.size()); + info.model = di.model; + info.hardware_version = di.hv; + info.firmware_version = (uint16_t(di.fvm) << 8) | uint16_t(di.fvs); + memcpy(info.serialnum, di.sn, SDK_SNLEN); + return RESULT_OK; + } + else if (data.size() == 3) //旧版设备信息（不带序列号） + { + info.model = YDLIDAR_SDM15; + info.hardware_version = data[0]; + info.firmware_version = (uint16_t(data[1]) << 8) | uint16_t(data[2]); + return RESULT_OK; + } + else + { + return RESULT_FAIL; + } +} + +} //end namespace ydlidar diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.h new file mode 100644 index 0000000..3d5bf64 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/SDMLidarDriver.h @@ -0,0 +1,453 @@ +/********************************************************************* +* 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. +*********************************************************************/ + +/** @page SDMLidarDriver + * SDMLidarDriver API + +
Library SDMLidarDriver +
File SDMLidarDriver.h +
Author ZhanYi [code at ydlidar com] +
Source https://github.com/ydlidar/YDLidar-SDK +
Version 1.0.0 +
+ This SDMLidarDriver support [TYPE_SDM](\ref LidarTypeID::TYPE_SDM) LiDAR + +* @copyright Copyright (c) @2015-2023 EAIBOT + Jump to the @link ::ydlidar::SDMLidarDriver @endlink interface documentation. +*/ +#ifndef SDM_YDLIDAR_DRIVER_H +#define SDM_YDLIDAR_DRIVER_H + +#include +#include +#include +#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_SDM_POINT_COUNT 1 +#define SDK_CMD_HEADFLAG0 0xAA //协议头标识1 +#define SDK_CMD_HEADFLAG1 0x55 //协议头标识2 +#define SDK_CMD_STARTSCAN 0x60 //开启测距 +#define SDK_CMD_STOPSCAN 0x61 //停止测距 +#define SDK_CMD_GETVERSION 0x62 //获取版本信息 +#define SDK_CMD_SELFCHECK 0x63 //自检 +#define SDK_CMD_SETFREQ 0x64 //设置输出频率 +#define SDK_CMD_SETFILTER 0x65 //设置滤波 +#define SDK_CMD_SETBAUDRATE 0x66 //设置串口波特率 +#define SDK_CMD_SETOUTPUT 0x67 //设置输出的数据格式 +#define SDK_CMD_RESET 0x68 //恢复出厂设置 +#define SDK_BUFFER_MAXLEN 100 //缓存长度 + +//设置1字节对齐 +#pragma pack(1) + +//SDM雷达协议头 +struct SdkSdmHead { + uint8_t head0 = 0; + uint8_t head1 = 0; + uint8_t cmd = 0; + uint8_t size = 0; +}; +#define SDKSDMHEADSIZE sizeof(SdkSdmHead) +//SDM雷达单点数据 +struct SdkSdmPc { + uint16_t dist = 0; //距离 + uint8_t intensity = 0; //强度 + uint8_t env = 0; //环境干扰数据 +}; +//SDM雷达一包点云数据 +struct SdkSdmPcs { + SdkSdmHead head; + SdkSdmPc point; //一包数据只有单点 + uint8_t cs = 0; +}; +#define SDKSDMPCSSIZE sizeof(SdkSdmPcs) +//SDM雷达设备信息 +struct SdkSdmDeviceInfo { + uint8_t model = 0; //雷达型号码 + uint8_t hv = 0; //硬件版本 + uint8_t fvm = 0; //固件主版本 + uint8_t fvs = 0; //固件子版本 + uint8_t sn[SDK_SNLEN] = {0}; //序列号 +}; +#define SDKSDMDEVICEINFOSIZE sizeof(SdkSdmDeviceInfo) + +#pragma pack() + + +using namespace std; + +namespace ydlidar +{ + +using namespace core; +using namespace core::serial; +using namespace core::base; + +/*! +* SDM操控类 +*/ +class SDMLidarDriver : public DriverInterface +{ +public: + //构造函数 + SDMLidarDriver(); + //析构函数 + virtual ~SDMLidarDriver(); + /*! + * @brief 连接雷达 \n + * 连接成功后，必须使用::disconnect函数关闭 + * @param[in] port 串口号 + * @param[in] baudrate 波特率，YDLIDAR-SDM 雷达波特率：961200 + * @return 返回连接状态 + * @retval 0 成功 + * @retval < 0 失败 + * @note连接成功后，必须使用::disconnect函数关闭 + * @see 函数::GSLidarDriver::disconnect (“::”是指定有连接功能,可以看文档里的disconnect变成绿,点击它可以跳转到disconnect.) + */ + result_t connect(const char *port, uint32_t baudrate); + + /*! + * @brief 断开雷达连接 + */ + void disconnect(); + + /*! + * @brief 获取当前SDK版本号 \n + * 静态函数 + * @return 返回当前SKD 版本号 + */ + virtual std::string getSDKVersion(); + + /*! + * @brief lidarPortList 获取雷达端口 + * @return 在线雷达列表 + */ + static std::map lidarPortList(); + + /*! + * @brief 扫图状态 \n + * @return 返回当前雷达扫图状态 + * @retval true 正在扫图 + * @retval false 扫图关闭 + */ + bool isscanning() const; + + /*! + * @brief 连接雷达状态 \n + * @return 返回连接状态 + * @retval true 成功 + * @retval false 失败 + */ + bool isconnected() const; + + /*! + * @brief 设置雷达异常自动重新连接 \n + * @param[in] enable 是否开启自动重连: + * true 开启 + * false 关闭 + */ + void setAutoReconnect(const bool &enable); + + /*! + * @brief 配置雷达地址 \n + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 配置成功 + * @retval RESULT_FAILE or RESULT_TIMEOUT 配置超时 + */ + result_t setDeviceAddress(uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 开启扫描 \n + * @param[in] force 扫描模式 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 开启成功 + * @retval RESULT_FAILE 开启失败 + * @note 只用开启一次成功即可 + */ + result_t startScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 关闭扫描 \n + * @return 返回执行结果 + * @retval RESULT_OK 关闭成功 + * @retval RESULT_FAILE 关闭失败 + */ + result_t stop(); + + /*! + * @brief 获取激光数据 \n + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + * @note 获取之前，必须使用::startScan函数开启扫描 + */ + result_t grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT) ; + + + /*! + * @brief 补偿激光角度 \n + * 把角度限制在0到360度之间 + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 补偿之前，必须使用::grabScanData函数获取激光数据成功 + */ + result_t ascendScanData(node_info *nodebuffer, size_t count); + + /*! + * @brief 重置激光雷达（恢复出厂设置） \n + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作, 如果在扫描中需先调用stop函数停止扫描 + */ + result_t reset(uint8_t addr, uint32_t timeout = DEFAULT_TIMEOUT); + + //设置开启或关闭滤波功能 + result_t enableFilter(bool yes=true); + + //设置扫描频率 + result_t setScanFreq(float sf, uint32_t timeout); + + protected: + + /*! + * @brief 创建解析雷达数据线程 \n + * @note 创建解析雷达数据线程之前，必须使用::startScan函数开启扫图成功 + */ + result_t createThread(); + + + /*! + * @brief 重新连接开启扫描 \n + * @param[in] force 扫描模式 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 开启成功 + * @retval RESULT_FAILE 开启失败 + * @note sdk 自动重新连接调用 + */ + result_t startAutoScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT) ; + + /*! + * @brief stopScan + * @param timeout + * @return + */ + result_t stopScan(uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief waitDevicePackage + * @param timeout + * @return + */ + result_t waitDevicePackage(uint32_t timeout = DEFAULT_TIMEOUT); + /*! + * @brief 解包激光数据 \n + * @param[in] node 解包后激光点信息 + * @param[in] timeout 超时时间 + */ + result_t waitPackage(node_info *node, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] nodebuffer 激光信息指针 + * @param[in] count 激光点数大小 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 失败 + */ + result_t waitScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 激光数据解析线程 \n + */ + int cacheScanData(); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] cmd 命名码 + * @param[in] payload payload + * @param[in] payloadsize payloadsize + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + */ + result_t sendCmd(uint8_t cmd, + const uint8_t *data = NULL, + size_t size = 0); + + /*! + * @brief 等待响应数据 \n + * @param[in] cmd 命令字 + * @param[out] data 响应数据 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 获取失败 + * @note 当timeout = -1 时, 将一直等待 + */ + result_t waitResp(uint8_t cmd, + uint32_t timeout = DEFAULT_TIMEOUT); + result_t waitResp(uint8_t cmd, + std::vector &data, + uint32_t timeout = DEFAULT_TIMEOUT); + /*! + * @brief 等待激光数据包头 \n + * @param[out] head 包头 + * @param[in] cmd 命令字 + * @param[out] data 响应数据 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 获取失败 + * @note 当timeout = -1 时, 将一直等待 + */ + // result_t waitResHeader(SdkSdmHead *head, + // uint8_t cmd, + // uint32_t timeout = DEFAULT_TIMEOUT); + // result_t waitResHeader(SdkSdmHead *head, + // uint8_t cmd, + // std::vector &data, + // uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 等待固定数量串口数据 \n + * @param[in] data_count 等待数据大小 + * @param[in] timeout 等待时间 + * @param[in] returned_size 实际数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 获取失败 + * @note 当timeout = -1 时, 将一直等待 + */ + result_t waitForData(size_t data_count, uint32_t timeout = DEFAULT_TIMEOUT, + size_t *returned_size = NULL); + + /*! + * @brief 获取串口数据 \n + * @param[in] data 数据指针 + * @param[in] size 数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + */ + result_t getData(uint8_t *data, size_t size); + + /*! + * @brief 串口发送数据 \n + * @param[in] data 发送数据指针 + * @param[in] size 数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 发送成功 + * @retval RESULT_FAILE 发送失败 + */ + result_t sendData(const uint8_t *data, size_t size); + /*! + * @brief 关闭数据获取通道 \n + */ + void disableDataGrabbing(); + /*! + * @brief 设置串口DTR \n + */ + void setDTR(); + /*! + * @brief 清除串口DTR \n + */ + void clearDTR(); + /*! + * @brief flushSerial + */ + void flushSerial(); + /*! + * @brief checkAutoConnecting + */ + result_t checkAutoConnecting(); + /** + * @brief 错误信息 + * @param isTCP TCP or UDP + * @return error information + */ + virtual const char *DescribeError(bool isTCP = false); + /** + * @brief GS2雷达没有健康信息\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); + /** + * @brief get Device information \n + * @param[in] info 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 &info, uint32_t timeout = DEFAULT_TIMEOUT); + +private: + serial::Serial *_serial = nullptr; //串口 + std::vector recvBuff; //一包数据缓存 + device_health health_; +}; + +} // namespace ydlidar + +#endif // SDM_YDLIDAR_DRIVER_H diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.cpp new file mode 100755 index 0000000..e2f2973 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.cpp @@ -0,0 +1,930 @@ +#include +#include +#include +#include "core/network/PassiveSocket.h" +#include "core/network/SimpleSocket.h" +#include "core/serial/common.h" +#include "core/common/ydlidar_help.h" +#include "TiaLidarDriver.h" + +//TIA雷达指令关键字 +#define P_TIA_SCANTYPE "scanType" //启停扫描 +#define P_TIA_SCANFREQ "target_speed_addr" //转速（寄存器值） +#define P_TIA_SAMPLERATE "measure_prf_addr" //采样率（寄存器值） +#define P_TIA_READ "Read" //读取 +#define P_TIA_CONFIG "config" //配置 +#define P_TIA_SCANFREQ2 "motorSpeed" //转速2（获取的值） +#define P_TIA_SAMPLERATE2 "samplerate" //采样率2（获取的值） +#define P_TIA_MODEL "model" //型号（获取的值） + +using namespace ydlidar; +using namespace ydlidar::core; +using namespace ydlidar::core::network; +using namespace ydlidar::core::base; +using namespace ydlidar::core::common; + + +TiaLidarDriver::TiaLidarDriver() +{ + socket_cmd = new core::network::CActiveSocket(CSimpleSocket::SocketTypeTcp); + socket_data = new core::network::CPassiveSocket(CSimpleSocket::SocketTypeUdp); + socket_data->SetSocketType(CSimpleSocket::SocketTypeUdp); + socket_cmd->SetConnectTimeout(DEFAULT_CONNECTION_TIMEOUT_SEC, + DEFAULT_CONNECTION_TIMEOUT_USEC); + scan_node_buf = new node_info[LIDAR_MAXNODES]; + scan_node_count = 0; + nodeIndex = 0; + retryCount = 0; + isAutoReconnect = true; + isAutoconnting = false; + m_Debug = false; + m_SupportMotorDtrCtrl = true; + + m_isScanning = false; + m_isConnected = false; + m_port = "192.168.0.11"; + m_baudrate = 8000; + m_SingleChannel = false; + m_LidarType = TYPE_TIA; + m_PointTime = 1e9 / 20000; +} + +TiaLidarDriver::~TiaLidarDriver() +{ + disconnect(); + + ScopedLocker l2(_lock2); + if (socket_data) { + delete socket_data; + socket_data = NULL; + } + ScopedLocker l(_lock); + if (socket_cmd) { + delete socket_cmd; + socket_cmd = NULL; + } + + if (scan_node_buf) { + delete[] scan_node_buf; + scan_node_buf = nullptr; + } +} + +result_t TiaLidarDriver::connect(const char *ip, uint32_t port) +{ + m_port = ip; + m_port2 = port; + m_isConnected = false; + + if (!configConnect(m_port.c_str(), m_port2)) + { + error("Fail to connect TCP [%s:%d],Error [%s]", + m_port.c_str(), m_port2, + DescribeError(true)); + return RESULT_FAIL; + } + info("Connect TCP [%s:%d]", m_port.c_str(), m_port2); + + if (!dataConnect(m_port.c_str(), m_baudrate)) + { + error("Fail to connect UDP [%s:%d],Error [%s]", + m_port.c_str(), m_baudrate, + DescribeError(false)); + configDisconnect(); + return RESULT_FAIL; + } + info("Connect UDP [%s:%d]", m_port.c_str(), m_baudrate); + + //设置转速 + setParam(P_TIA_SCANFREQ2, double(m_ScanFreq)); + //设置采样率 + setParam(P_TIA_SAMPLERATE2, double(m_SampleRate)); + + m_isConnected = true; + return RESULT_OK; +} + +bool TiaLidarDriver::isconnected() const { + return m_isConnected; +} + +bool TiaLidarDriver::isscanning() const { + return m_isScanning; +} + +void TiaLidarDriver::setIntensities(const int &i) { + m_intensities = i; +} + +void TiaLidarDriver::setAutoReconnect(const bool &enable) { + isAutoReconnect = enable; +} + +const char *TiaLidarDriver::DescribeError(bool isTCP) +{ + if (isTCP) + { + ScopedLocker lock(_lock); + return socket_cmd != NULL ? socket_cmd->DescribeError() : "NO Socket"; + } + else + { + ScopedLocker lock(_lock2); + return socket_data != NULL ? socket_data->DescribeError() : "NO Socket"; + } +} + +bool TiaLidarDriver::configConnect(const char *ip, int port) +{ + ScopedLocker lock(_lock); + + if (!socket_cmd) + return false; + if (!socket_cmd->IsSocketValid()) { + if (!socket_cmd->Initialize()) { + return false; + } + } else { + return socket_cmd->IsSocketValid(); + } + +// socket_cmd->SetNonblocking(); + socket_cmd->bindport(ip, port); + if (!socket_cmd->open()) +// if (!socket_cmd->Open(ip, port)) //此方式会失败 + { + socket_cmd->Close(); + return false; + } + + socket_cmd->SetReceiveTimeout( + SDK_TIMEOUT / 1000, + (SDK_TIMEOUT % 1000) * 1000); + socket_cmd->SetSendTimeout( + SDK_TIMEOUT / 1000, + (SDK_TIMEOUT % 1000) * 1000); + socket_cmd->SetBlocking(); + + return socket_cmd->IsSocketValid(); +} + +void TiaLidarDriver::configDisconnect() +{ + ScopedLocker lock(_lock); + if (!socket_cmd) + return; + if (socket_cmd->IsSocketValid()) + { + socket_cmd->Close(); + info("Disconnect TCP"); + } +} + +void TiaLidarDriver::disconnect() +{ + stopScan(); + + dataDisconnect(); + configDisconnect(); +} + +result_t TiaLidarDriver::getHealth(device_health &health, uint32_t timeout) +{ + UNUSED(timeout); + health.error_code = 0; + health.status = 0; + return RESULT_OK; +} + +result_t TiaLidarDriver::getDeviceInfo(device_info &di, uint32_t timeout) +{ + UNUSED(timeout); + + result_t ret = RESULT_FAIL; + + //设置默认值 + m_model = YDLIDAR_TIA; + + //获取设备信息 + cJSON *o = nullptr; + if (getParams(P_TIA_CONFIG, o)) + { + //雷达型号 + cJSON *jm = cJSON_GetObjectItem(o, P_TIA_MODEL); + if (cJSON_IsNumber(jm)) + { + m_model = jm->valueint; //型号码 + ret = RESULT_OK; + } + } + cJSON_Delete(o); + + di.firmware_version = 0; + di.hardware_version = 0; + di.model = m_model; + +// debug("TIA型号[%d]", m_model); + + return ret; +} + +// result_t TiaLidarDriver::start(bool force, uint32_t timeout) +// { +// UNUSED(force); +// UNUSED(timeout); + +// result_t ret = RESULT_FAIL; + +// if (m_isScanning) +// return RESULT_OK; + +// if (!IS_OK(startScan()) && +// !IS_OK(startScan())) +// return RESULT_FAIL; + +// ret = createThread(); + +// return ret; +// } + +// result_t TiaLidarDriver::stop() +// { +// //标记防止自动重连 +// if (isAutoconnting) +// isAutoconnting = false; + +// //如果未开启扫描，则直接返回 +// if (!isscanning()) +// return RESULT_OK; +// m_isScanning = false; + +// deleteThread(); + +// if (!IS_OK(stopScan()) && +// !IS_OK(stopScan())) +// return RESULT_FAIL; + +// return RESULT_OK; +// } + +bool TiaLidarDriver::createThread() +{ + m_thread = new std::thread(&TiaLidarDriver::parseScanDataThread, this); + if (!m_thread) + { + error("Fail to create data thread"); + return false; + } + info("Create TIA thread 0x%X", m_thread->get_id()); + + // m_thread2 = new std::thread(&TiaLidarDriver::parseParamInfoThread, this); + // if (!m_thread2) + // { + // warn("Fail to create TIA parameter thread"); + // } + + return true; +} + +result_t TiaLidarDriver::startScan(bool force, uint32_t timeout) +{ + UNUSED(force); + UNUSED(timeout); + + bool ret = setParam(P_TIA_SCANTYPE, 0); + + ret &= createThread(); + + return ret ? RESULT_OK : RESULT_FAIL; +} + +result_t TiaLidarDriver::stopScan(uint32_t timeout) +{ + UNUSED(timeout); + + //标记防止自动重连 + if (isAutoconnting) + isAutoconnting = false; + //如果未开启扫描，则直接返回 + if (!isscanning()) + return RESULT_OK; + m_isScanning = false; + + deleteThread(); + + //停止扫描 + bool ret = setParam(P_TIA_SCANTYPE, -1); + + return ret ? RESULT_OK : RESULT_FAIL; +} + +bool TiaLidarDriver::dataConnect(const char *ip, int port) +{ + UNUSED(ip); + + ScopedLocker l(_lock2); + if (!socket_data) + return false; + + if (!socket_data->IsSocketValid()) + { + if (socket_data->Initialize()) + { + if (!socket_data->Listen(NULL, port)) //NULL + { + socket_data->Close(); + return false; + } + + socket_data->SetReceiveTimeout(SDK_TIMEOUT / 1000, + (SDK_TIMEOUT % 1000) * 1000); + } + } + + return socket_data->IsSocketValid(); +} + +void TiaLidarDriver::dataDisconnect() +{ + ScopedLocker l(_lock2); + if (!socket_data) + return; + if (socket_data->IsSocketValid()) + { + socket_data->Close(); + info("Disconnect UDP"); + } +} + +void TiaLidarDriver::deleteThread() +{ + if (m_isScanning) + { + m_isScanning = false; + ScopedLocker l(_lock); + _dataEvent.set(); + } + if (m_thread) + { + if (m_thread->joinable()) + m_thread->join(); + delete m_thread; + m_thread = nullptr; + } + if (m_thread2) + { + if (m_thread2->joinable()) + m_thread2->join(); + delete m_thread2; + m_thread2 = nullptr; + } +} + +result_t TiaLidarDriver::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; + return RESULT_TIMEOUT; + + case Event::EVENT_OK: + { + if (scan_node_count == 0) + { + ret = RESULT_FAIL; + } + else + { + ScopedLocker l(_lock); + size_t size_to_copy = std::min(count, scan_node_count); + memcpy(nodebuffer, scan_node_buf, size_to_copy * sizeof(node_info)); + count = size_to_copy; + scan_node_count = 0; + ret = RESULT_OK; + } + _dataEvent.set(false); //重置事件状态 + break; + } + default: + count = 0; + return RESULT_FAIL; + } + + return ret; +} + +result_t TiaLidarDriver::getScanFrequency( + scan_frequency &frequency, + uint32_t timeout) +{ + UNUSED(frequency); + UNUSED(timeout); + + result_t ans = RESULT_FAIL; + return ans; +} + +result_t TiaLidarDriver::getSamplingRate( + sampling_rate &rate, + uint32_t timeout) +{ + UNUSED(rate); + UNUSED(timeout); + return RESULT_FAIL; +} + +result_t TiaLidarDriver::setSamplingRate( + sampling_rate &rate, + uint32_t timeout) +{ + UNUSED(rate); + UNUSED(timeout); + return RESULT_FAIL; +} + +result_t TiaLidarDriver::checkAutoConnecting() +{ + result_t ans = RESULT_FAIL; + isAutoconnting = true; //标记正在重连中 + if (m_driverErrno != BlockError) + setDriverError(TimeoutError); + + while (isAutoReconnect && isscanning()) + { + //先断开连接 + { + configDisconnect(); + dataDisconnect(); + } + //重新连接 + while (isAutoconnting && + !IS_OK(connect(m_port.c_str(), m_baudrate))) + { + setDriverError(NotOpenError); + delay(300); //延时 + } + if (!isAutoconnting) + { + m_isScanning = false; + return RESULT_FAIL; + } + //重新启动雷达 + if (isconnected() && + isAutoconnting) + { + ans = startScan(); + if (IS_OK(ans)) + { + if (getDriverError() == DeviceNotFoundError) + setDriverError(NoError); + isAutoconnting = false; + return ans; + } + else + { + setDriverError(DeviceNotFoundError); + delay(300); //延时 + } + } + } + + isAutoconnting = false; + setDriverError(NoError); + return RESULT_FAIL; +} + +int TiaLidarDriver::parseScanDataThread() +{ + node_info local_scan[LIDAR_MAXNODES]; + node_info local_buf[TIA_PACKMAXNODES]; + size_t count = TIA_PACKMAXNODES; + size_t scan_count = 0; + result_t ans = RESULT_FAIL; + int timeout_count = 0; + + memset(local_scan, 0, sizeof(local_scan)); + lastZeroTime = getms(); + lastPackIndex = 0; + + m_isScanning = true; + + while (m_isScanning) + { + count = 0; + memset(local_buf, 0, sizeof(local_buf)); + ans = getScanData(local_buf, count); + if (!IS_OK(ans)) + { + if (timeout_count > DEFAULT_TIMEOUT_COUNT) + { + if (!isAutoReconnect) + { + error("Exit scanning thread!"); + m_isScanning = false; + return RESULT_FAIL; + } + else + { + ans = checkAutoConnecting(); //重连 + if (IS_OK(ans)) { + timeout_count = 0; + local_scan[0].sync = NODE_UNSYNC; + } else { + m_isScanning = false; + return RESULT_FAIL; + } + } + } + else + { + timeout_count ++; + local_scan[0].sync = NODE_UNSYNC; + error("Timout count [%d]", timeout_count); + } + } + else + { + timeout_count = 0; + + for (size_t i = 0; i < count; ++i) + { + if (NODE_SYNC == local_buf[i].sync) + { + { + ScopedLocker l(_lock); + memcpy(scan_node_buf, local_scan, scan_count * SDKNODESIZE); + scan_node_count = scan_count; + _dataEvent.set(); + } + scan_count = 0; + } + + local_scan[scan_count++] = local_buf[i]; + if (scan_count >= LIDAR_MAXNODES) + scan_count = 0; + } + } + } + + m_isScanning = false; + + return RESULT_OK; +} + +int TiaLidarDriver::parseParamInfoThread() +{ + //定时获取转速和采样率等信息 + while (m_isScanning) + { + { + cJSON *o = nullptr; + if (getParams(P_TIA_CONFIG, o)) + { + cJSON *sf = cJSON_GetObjectItem(o, P_TIA_SCANFREQ2); + if (cJSON_IsNumber(sf)) + m_param.scanFreq = sf->valuedouble / 100; + cJSON *sr = cJSON_GetObjectItem(o, P_TIA_SAMPLERATE2); + if (cJSON_IsNumber(sr)) + m_param.sampleRate = sr->valuedouble / 100; + debug("ScanFreq: %f SampleRate: %f", + m_param.scanFreq, m_param.sampleRate); + } + cJSON_Delete(o); + } + int count = 0; + while (count++ < 4 && m_isScanning) + delay(TIMEOUT_500); + } + + return RESULT_OK; +} + +bool TiaLidarDriver::setParams(const cJSON* json) +{ + bool ret = true; + if (json->child) + { + //遍历所有值 + cJSON *child = json->child; + while (child) + { + cJSON *o = cJSON_CreateObject(); + cJSON_AddItemToObject(o, child->string, child); + ret = setParam(o); + if (!ret) + break; + child = child->next; + } + } + + return ret; +} + +bool TiaLidarDriver::setParam( + const std::string &key, + const float &value) +{ + cJSON *json = cJSON_CreateObject(); + + if (P_TIA_SCANFREQ == key) //设置寄存器值 + { + //390625/设置值=转速（单位Hz） + float v = value; + if (v > 0 && v <= 200) + cJSON_AddNumberToObject(json, key.data(), int(390625 / v)); + else + warn("TIA scanning frequency [%.02fHz] setting is incorrect", v); + } + else if (P_TIA_SAMPLERATE == key) //设置寄存器值 + { + //100000000/设置值=采样率（单位K/s） + float v = value; + if (v > 0 && v <= 300) + cJSON_AddNumberToObject(json, key.data(), int(100000 / v)); + else + warn("TIA sampling rate [%.02fK/s] setting is incorrect", v); + } + else if (P_TIA_SCANFREQ2 == key) + { + //转速（单位Hz） + int v = value; + if (v > 0 && v <= 200) + cJSON_AddNumberToObject(json, key.data(), v); + else + warn("TIA scanning frequency [%dHz] setting is incorrect", v); + } + else if (P_TIA_SAMPLERATE2 == key) + { + //100000000/设置值=采样率（单位K/s） + int v = value; + if (v > 0 && v <= 300) + cJSON_AddNumberToObject(json, key.data(), v); + else + warn("TIA sampling rate [%dK/s] setting is incorrect", v); + } + else + { + cJSON_AddNumberToObject(json, key.data(), value); + } + + return setParam(json); +} + +bool TiaLidarDriver::setParam(cJSON *json) +{ + if (!sendData(json)) + return false; + + cJSON *js = nullptr; + if (!waitResp(js, TIMEOUT_500)) + return false; + bool ret = cJSON_Compare(json, js, cJSON_bool(0)); + cJSON_Delete(js); //释放内存 + if (!ret) + { + warn("Fail to set lidar parameter,Error to response"); + return false; + } + + return true; +} + +bool TiaLidarDriver::getParams(const std::string &key, cJSON* &value) +{ + //组装发送数据 + cJSON* json = cJSON_CreateObject(); + cJSON_AddStringToObject(json, P_TIA_READ, key.data()); + if (!sendData(json)) + return false; + + if (!waitResp(value, TIMEOUT_1S)) + return false; + return true; +} + +bool TiaLidarDriver::sendData(cJSON* json) +{ + if (!json) + return false; + char* ss = cJSON_PrintUnformatted(json); //发送命令需要使用紧凑json格式 + std::string sd(ss); + free(ss); + cJSON_Delete(json); //释放内存 + //发送数据 + ScopedLocker l(_lock); + int32_t rs = socket_cmd->Send( + reinterpret_cast(sd.data()), sd.size()); + if (m_Debug) + debug("[SEND] %s", sd.c_str()); + if (rs <= 0) + { + warn("Fail to send TCP data"); + return false; + } + + return true; +} + +bool TiaLidarDriver::waitResp(cJSON* &json, uint32_t timeout) +{ + char buff[300] = {0}; + ScopedLocker l(_lock); + if (socket_cmd->Select(timeout / 1000, timeout * 1000)) + { + int32_t rs = socket_cmd->Receive(sizeof(buff), + reinterpret_cast(buff)); + if (rs > 0) //有响应数据时 + { + std::string rd(buff, rs); + if (m_Debug) + debug("[RECV] %s", rd.c_str()); + //解析JSON数据 + json = cJSON_Parse(rd.data()); + if (!json) + { + warn("Fail to wait response,Error json format"); + return false; + } + return true; + } + } + + warn("Waiting for response timeout [%u]ms", timeout); + return false; +} + +bool TiaLidarDriver::correct(float &angle, float &dist) +{ + return m_param.left.correct(angle, dist) || + m_param.right.correct(angle, dist); +} + +result_t TiaLidarDriver::getScanData(node_info* nodes, size_t& count) +{ + int32_t rs = 0; + memset(m_buff, 0, TIA_PACKMAXBUFFS); + { + ScopedLocker l(_lock2); + if (!socket_data->IsSocketValid()) + return RESULT_FAIL; + rs = socket_data->Receive(sizeof(m_buff), + reinterpret_cast(m_buff)); + if (rs <= 0) + { + warn("Fail to recv UDP data"); + return RESULT_TIMEOUT; + } + else if (rs < TIA_PACKMAXBUFFS) + { + warn("The packet length [%d] < the expected value [%d]", + rs, TIA_PACKMAXBUFFS); + return RESULT_FAIL; + } + } + uint8_t* data = m_buff; + if (m_Debug) + debugh(m_buff, rs); + + uint8_t packIndex = 0; //当前包序号 + uint64_t stamp = 0; //时间戳 + int idx = 0; //数据索引 + //解析时间戳和包序号 + idx = TIA_PACKMAXBUFFS - 8; + if (YDLIDAR_TIA_H == m_model || + YDLIDAR_TIA_X == m_model) + { + if (rs >= TIA_PACKMAXBUFFS) + { + stamp = getBigValue(&data[idx], 4); //时间戳 + idx += 4; + packIndex = getBigValue(&data[idx], 1); //包序号 + idx += 1; + } + } + else //YDLIDAR_TIA == m_model + { + if (rs >= TIA_PACKMAXBUFFS2) + { + stamp = getBigValue(&data[idx], 4) * 1000; //时间戳（秒） + idx += 4; + stamp += getBigValue(&data[idx], 4); //时间戳（毫秒） + stamp *= 1000000; //毫秒转纳秒 + idx += 4; + packIndex = uint8_t(getBigValue(&data[idx], 1) & 0x0F); //1字节低4位是包序号 + idx += 1; + } + } + if (abs(packIndex - lastPackIndex) > 1 && + packIndex != 0 && + lastPackIndex != 0) + warn("Data packet lost,current packet index [%u],last packet index [%u]", + packIndex, lastPackIndex); + lastPackIndex = packIndex; + + //大端序 + uint16_t h = 0; //头 + uint16_t a = 0; //角度 + uint8_t as = 0; //角度增量 + float sa = .0; + float ea = .0; + uint8_t p = 0; //强度 + uint16_t d = 0; //距离 + int index = 0; //点序号 + bool invalid = true; //无效标记 + idx = 0; //重置位置 + for (int i=0; i 1.0 * 100.0) + { + nodes[index].sync = NODE_SYNC; + //计算转速 +// nodes[index].scanFreq = 1e9 / (getTime() - lastZeroTime); + //赋值获取到的实际转速 + nodes[index].scanFreq = m_ScanFreq; //m_param.scanFreq; + lastZeroTime = getTime(); + if (m_Debug) + debug("Zero time [%llu]ns", lastZeroTime); + } + else + { + nodes[index].sync = NODE_UNSYNC; + } + //保留当前角度值 + m_lastAngle = a; + + //赋值 + nodes[index].index = 0; + nodes[index].is = (as >> 6); + nodes[index].angle = float(a) / 100.0 * 128; //角度值放大64倍以便和其它雷达保持一致 + nodes[index].dist = d; + nodes[index].qual = p; + + //如果是TIA-X，需要根据反射镜参数对指定角度范围内的角度和距离进行修正 + if (YDLIDAR_TIA_X == m_model) + { + float angle = a / 100.0; + float dist = d; + if (correct(angle, dist)) + { + nodes[index].angle = angle * 128; + nodes[index].dist = dist; + +// SeLog::debug("点%d 原[%.02f,%.0f] 改[%.02f,%.0f]", +// index, a / 100.0, float(d), angle, dist); + } + } + +// SeLog::debug("点%d [a:%.02f+%.02f d:%u i:%u]", +// index, a / 100.0, (as && 0x3F) / 100.0, d, p); + + index ++; + + //如果是无效数据，需要终止本次解析 + if (invalid) + break; + } + if (invalid) + break; + } + if (m_Debug) + debug("Angle range [%.02f~%.02f] stamp [%lu]ns index [%u]", + sa, ea, stamp, packIndex); + + count = index; + + return RESULT_OK; +} \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.h new file mode 100755 index 0000000..4c8e93d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/TiaLidarDriver.h @@ -0,0 +1,248 @@ +#pragma once +#include +#include +#include "core/base/thread.h" +#include "core/base/locker.h" +#include "core/common/ydlidar_protocol.h" +#include "core/common/ydlidar_datatype.h" +#include "core/common/DriverInterface.h" +#include "core/json/cJSON.h" + + +//雷达参数 +struct EaiLidarBaseParam +{ + int type = 0; //雷达类型 +}; +//TIA雷达参数 +struct EaiTiaParam : public EaiLidarBaseParam +{ + std::string ip = "192.168.0.11"; //IP地址 + int port = 8090; //端口号 + std::string mask = "255.255.255.0"; //子网掩码 + std::string gateway = "192.168.0.1"; //网关 + std::string mac; //MAC地址 + + float scanFreq = .0f; //扫描频率，单位Hz + float sampleRate = .0f; //采样率，单位K/s +}; +//TIA-X雷达参数 +struct EaiTiaXParamItem //项 +{ + float angle = 45.0f; //反射镜安装角度（单位°） + float dist = 30.0f; //反射镜安装距离（单位mm） + float opd = 0.0f; //光程差（单位mm） + float minAngle = 60.0f; //反射镜起始角度（单位°） + float maxAngle = 100.0f; //反射镜结束角度（单位°） + + //根据参数修正角度和距离 + bool correct(float &angle, float &dist) const { + //距离无效直接返回 + if (int(dist) <= 0) + return false; + bool has = angle > SDK_ANGLE180; //超过180°的标记 + //先判断角度值是否有效 + if (angle >= minAngle && + angle <= maxAngle) //左反射镜 + { + if (has) + angle = SDK_ANGLE360 - angle; + float beta = SDK_ANGLE180 - this->angle - angle; + float a = this->dist / sin(beta * M_PI / SDK_ANGLE180); //边长a + float b = dist - a; //边长b + float C = (SDK_ANGLE180 - 2 * beta) * M_PI / SDK_ANGLE180; //角C（弧度） + float c = sqrt(a * a + b * b - 2 * a * b * cos(C)); //边长c + //余弦定理计算 + float B = acos((a * a + c * c - b * b) / (2 * a * c)) * + SDK_ANGLE180 / M_PI; //角B（角度） + if (has) + angle = SDK_ANGLE360 - (B + angle); + else + angle += B; + + dist = c + this->opd; //距离增加光程差值 + + return true; + } + return false; + } +}; +struct EaiTiaXParam : public EaiTiaParam +{ + EaiTiaXParamItem left; //左反射镜参数 + EaiTiaXParamItem right; //右反射镜参数 +}; + +namespace ydlidar { +namespace core { +namespace network { +class CActiveSocket; +class CPassiveSocket; +} +} + +using namespace core::common; +using namespace core::base; +using namespace core::network; + +class TiaLidarDriver : public DriverInterface +{ +public: + explicit TiaLidarDriver(); + ~TiaLidarDriver(); + + //连接雷达 + virtual result_t connect(const char *port_path, uint32_t baudrate = 8000); + //错误描述 + virtual const char *DescribeError(bool isTCP = true); + //断连雷达 + virtual void disconnect(); + //是否在扫描状态 + virtual bool isscanning() const; + //是否在连接状态 + virtual bool isconnected() const; + + /*! + * @brief 设置雷达是否带信号质量 \n + * 连接成功后，必须使用::disconnect函数关闭 + * @param[in] isintensities 是否带信号质量: + * true 带信号质量 + * false 无信号质量 + * @note只有S4B(波特率是153600)雷达支持带信号质量, 别的型号雷达暂不支持 + */ + virtual void setIntensities(const int &i); + + /*! + * @brief 设置雷达异常自动重新连接 \n + * @param[in] enable 是否开启自动重连: + * true 开启 + * false 关闭 + */ + virtual void setAutoReconnect(const bool &enable); + //获取雷达设备健康状态 + virtual result_t getHealth( + device_health &health, + uint32_t timeout = SDK_TIMEOUT); + //获取设备信息 + virtual result_t getDeviceInfo( + device_info &info, + uint32_t timeout = SDK_TIMEOUT); + // //启动雷达 + // virtual result_t start( + // bool force = false, + // uint32_t timeout = SDK_TIMEOUT); + // //停止雷达 + // virtual result_t stop(); + /*! + * @brief 获取激光数据 \n + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + * @note 获取之前，必须使用::startScan函数开启扫描 + */ + virtual result_t grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = SDK_TIMEOUT) ; + /*! + * @brief 获取激光雷达当前扫描频率 \n + * @param[in] frequency 扫描频率 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作 + */ + virtual result_t getScanFrequency(scan_frequency &frequency, + uint32_t timeout = SDK_TIMEOUT); + + /*! + * @brief 获取激光雷达当前采样频率 \n + * @param[in] frequency 采样频率 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作 + */ + virtual result_t getSamplingRate( + sampling_rate &rate, + uint32_t timeout = SDK_TIMEOUT); + + /*! + * @brief 设置激光雷达当前采样频率 \n + * @param[in] rate 　　　采样频率 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作 + */ + virtual result_t setSamplingRate( + sampling_rate &rate, + uint32_t timeout = SDK_TIMEOUT); + +private: + //连接TCP + bool configConnect(const char *lidarIP, int tcpPort = 8090); + //断连TCP + void configDisconnect(); + //连接UDP + bool dataConnect(const char *lidarIP, int localPort = 8000); + //断连UDP + void dataDisconnect(); + //启动扫描 + virtual result_t startScan( + bool force = false, + uint32_t timeout = SDK_TIMEOUT); + //停止扫描 + virtual result_t stopScan( + uint32_t timeout = SDK_TIMEOUT); + //创建线程 + bool createThread(); + //销毁线程 + void deleteThread(); + + result_t getScanData(node_info* nodes, size_t& count); + //检查自动连接 + result_t checkAutoConnecting(); + + //解析扫描数据线程函数 + int parseScanDataThread(); + //定时获取转速等信息线程函数 + int parseParamInfoThread(); + + //设置多个参数 + bool setParams(const cJSON* json); + //设置单个参数 + bool setParam(const std::string& key, const float& value); + bool setParam(cJSON* json); + //获取多个参数 + bool getParams(const std::string& key, cJSON* &value); + //发送数据 + bool sendData(cJSON* json); + //等待响应 + bool waitResp(cJSON* &json, uint32_t timeout = SDK_TIMEOUT); + +private: + //TIA-X修正角度和距离 + bool correct(float& a, float& d); + +private: + int m_model = YDLIDAR_TIA; //雷达型号 + float m_lastAngle = 0.f; + int m_port2 = 9000; + /* Sockets for ydlidar */ + CActiveSocket *socket_cmd = nullptr; + CPassiveSocket *socket_data = nullptr; + uint8_t m_buff[TIA_PACKMAXBUFFS2]; //缓存 + uint64_t lastZeroTime = 0; //上一零位点时间 + uint8_t lastPackIndex = 0; //上一包包序号 + //TIA-X专用参数 + EaiTiaXParam m_param; //参数 + Thread _thread2; //参数线程 + Locker _lock2; //操作串口或网络的锁（不支持嵌套） +}; + +} //ydlidar diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.cpp new file mode 100644 index 0000000..103f477 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.cpp @@ -0,0 +1,2808 @@ +// +// 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. +// +#include +#include +#include "core/common/ydlidar_help.h" +#include "core/serial/common.h" +#include "core/serial/serial.h" +#include "core/network/ActiveSocket.h" +#include "YDlidarDriver.h" +#include "ydlidar_config.h" + +using namespace impl; + +namespace ydlidar +{ + using namespace core::serial; + using namespace core::common; + using namespace core::network; + + YDlidarDriver::YDlidarDriver(uint8_t type) : _serial(NULL) + { + m_isConnected = false; + m_isScanning = false; + // 串口配置参数 + m_intensities = false; + isAutoReconnect = true; + isAutoconnting = false; + m_baudrate = 230400; + m_SupportMotorDtrCtrl = true; + m_HeartBeat = false; + scan_node_count = 0; + sample_rate = 5000; + m_PointTime = 1e9 / 5000; + trans_delay = 0; + scan_frequence = 0; + m_sampling_rate = -1; + model = YDLIDAR_S2PRO; + retryCount = 0; + has_device_header = false; + m_SingleChannel = false; + m_LidarType = TYPE_TRIANGLE; + m_DeviceType = type; + + // 解析参数 + PackageSampleBytes = 2; + IntervalSampleAngle = 0.0; + FirstSampleAngle = 0; + LastSampleAngle = 0; + CheckSum = 0; + CheckSumCal = 0; + SampleNumlAndCTCal = 0; + LastSampleAngleCal = 0; + CheckSumResult = true; + Valu8Tou16 = 0; + ct = CT_Normal; + nowPackageNum = 0; + package_Sample_Num = 0; + + last_device_byte = 0x00; + asyncRecvPos = 0; + async_size = 0; + headerBuffer = reinterpret_cast(&header_); + healthBuffer = reinterpret_cast(&health_); + nodeIndex = 0; + globalRecvBuffer = new uint8_t[sizeof(tri_node_package)]; + scan_node_buf = new node_info[MAX_SCAN_NODES]; + package_index = 0; + has_package_error = false; + + get_device_health_success = false; + m_HasDeviceInfo = EPT_None; + IntervalSampleAngle_LastPackage = 0.0; + m_heartbeat_ts = getms(); + m_BlockRevSize = 0; + } + + YDlidarDriver::~YDlidarDriver() + { + disableDataGrabbing(); + { + ScopedLocker l(_cmd_lock); + if (_serial) + { + if (_serial->isOpen()) + { + _serial->flush(); + _serial->closePort(); + } + } + if (_serial) + { + delete _serial; + _serial = NULL; + } + } + + { + ScopedLocker l(_lock); + if (globalRecvBuffer) + { + delete[] globalRecvBuffer; + globalRecvBuffer = NULL; + } + + if (scan_node_buf) + { + delete[] scan_node_buf; + scan_node_buf = NULL; + } + } + } + + result_t YDlidarDriver::connect(const char *port_path, uint32_t baudrate) + { + m_baudrate = baudrate; + m_port = string(port_path); + + { + ScopedLocker l(_cmd_lock); + if (!_serial) + { + if (m_DeviceType == YDLIDAR_TYPE_TCP) + { + _serial = new CActiveSocket(); + } + else + { + _serial = new serial::Serial( + m_port, m_baudrate, + serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT/2)); + } + _serial->bindport(port_path, baudrate); + } + if (!_serial->open()) + { + setDriverError(NotOpenError); + return RESULT_FAIL; + } + m_isConnected = true; + } + + //如果是双通雷达，需要先停止 + if (!m_SingleChannel) + { + info("Stop Lidar"); + stop(); + } + + return RESULT_OK; + } + + const char *YDlidarDriver::DescribeError(bool isTCP) + { + char const *value = ""; + + if (_serial) + { + return _serial->DescribeError(); + } + + return value; + } + + void YDlidarDriver::setDTR() + { + if (!m_isConnected) + { + return; + } + + if (_serial) + { + _serial->setDTR(1); + } + } + + void YDlidarDriver::clearDTR() + { + if (!m_isConnected) + { + return; + } + + if (_serial) + { + _serial->setDTR(0); + } + } + void YDlidarDriver::flushSerial() + { + if (!m_isConnected) + { + return; + } + + size_t len = _serial->available(); + + if (len) + { + _serial->readSize(len); + } + + delay(20); + } + + void YDlidarDriver::disconnect() + { + isAutoReconnect = false; + + if (!m_isConnected) + { + return; + } + + stop(); + delay(10); + + ScopedLocker l(_cmd_lock); + if (_serial) + { + if (_serial->isOpen()) + { + _serial->closePort(); + } + } + + m_isConnected = false; + } + + void YDlidarDriver::disableDataGrabbing() + { + m_isScanning = false; + _dataEvent.set(); + // _thread.join(); + if (m_thread) + { + if (m_thread->joinable()) + m_thread->join(); + delete m_thread; + m_thread = nullptr; + } + } + + bool YDlidarDriver::isscanning() const + { + return m_isScanning; + } + + bool YDlidarDriver::isconnected() const + { + return m_isConnected; + } + + result_t YDlidarDriver::sendCommand(uint8_t cmd, const void *payload, + size_t payloadsize) + { + uint8_t pkt_header[10]; + cmd_packet *header = reinterpret_cast(pkt_header); + uint8_t checksum = 0; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + if (payloadsize && payload) + { + cmd |= LIDAR_CMDFLAG_HAS_PAYLOAD; + } + + header->syncByte = LIDAR_CMD_SYNC_BYTE; + header->cmd_flag = cmd; + sendData(pkt_header, 2); + + if ((cmd & LIDAR_CMDFLAG_HAS_PAYLOAD) && payloadsize && payload) + { + checksum ^= LIDAR_CMD_SYNC_BYTE; + checksum ^= cmd; + checksum ^= (payloadsize & 0xFF); + + for (size_t pos = 0; pos < payloadsize; ++pos) + { + checksum ^= ((uint8_t *)payload)[pos]; + } + + uint8_t sizebyte = (uint8_t)(payloadsize); + sendData(&sizebyte, 1); + + sendData((const uint8_t *)payload, sizebyte); + + sendData(&checksum, 1); + } + + return RESULT_OK; + } + + result_t YDlidarDriver::sendData(const uint8_t *data, size_t size) + { + if (!m_isConnected) + { + return RESULT_FAIL; + } + + if (data == NULL || size == 0) + { + return RESULT_FAIL; + } + + size_t r = 0; + + while (size) + { + r = _serial->writeData(data, size); + + if (r < 1) + { + return RESULT_FAIL; + } + + if (m_Debug) + { + debugh(data, r); + } + + size -= r; + data += r; + } + + return RESULT_OK; + } + + result_t YDlidarDriver::getData(uint8_t *data, size_t size) + { + if (!m_isConnected) + return RESULT_FAIL; + + size_t r = 0; + while (size) + { + r = _serial->readData(data, size); + if (r < 1) + { + return RESULT_FAIL; + } + + if (m_Debug) + { + debugh(data, r); + } + + size -= r; + data += r; + } + + return RESULT_OK; + } + + result_t YDlidarDriver::waitResponseHeader(lidar_ans_header *header, + uint32_t timeout) + { + int recvPos = 0; + uint32_t startTs = getms(); + uint8_t recvBuffer[sizeof(lidar_ans_header)]; + uint8_t *headerBuffer = reinterpret_cast(header); + uint32_t waitTime = 0; + has_device_header = false; + last_device_byte = 0x00; + + while ((waitTime = getms() - startTs) <= timeout) + { + size_t remainSize = sizeof(lidar_ans_header) - recvPos; + size_t recvSize = 0; + result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + + if (!IS_OK(ans)) + { + return ans; + } + + if (recvSize > remainSize) + { + recvSize = remainSize; + } + + ans = getData(recvBuffer, recvSize); + + if (IS_FAIL(ans)) + { + return RESULT_FAIL; + } + + for (size_t pos = 0; pos < recvSize; ++pos) + { + uint8_t currentByte = recvBuffer[pos]; + + switch (recvPos) + { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) + { + if (last_device_byte == (PH & 0xFF) && currentByte == (PH >> 8)) + { + has_device_header = true; + } + + last_device_byte = currentByte; + continue; + } + + break; + + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE2) + { + last_device_byte = currentByte; + recvPos = 0; + continue; + } + + break; + } + + headerBuffer[recvPos++] = currentByte; + last_device_byte = currentByte; + + if (recvPos == sizeof(lidar_ans_header)) + { + return RESULT_OK; + } + } + } + + return RESULT_FAIL; + } + + result_t YDlidarDriver::waitForData(size_t data_count, uint32_t timeout, + size_t *returned_size) + { + size_t length = 0; + + if (returned_size == NULL) + { + returned_size = (size_t *)&length; + } + + return (result_t)_serial->waitfordata(data_count, timeout, returned_size); + } + + result_t YDlidarDriver::checkAutoConnecting(bool serialError) + { + result_t ans = RESULT_FAIL; + m_InvalidNodeCount = 0; + if (m_driverErrno != BlockError) + setDriverError(TimeoutError); + + while (isAutoReconnect && isscanning()) + { + //先断开串口连接 + { + ScopedLocker l(_cmd_lock); + if (_serial) + { + if (_serial->isOpen()) + { + //根据串口缓存数据大小判断错误类型（有点离谱） + size_t buffer_size = _serial->available(); + m_BufferSize += buffer_size; + if (m_BufferSize && m_BufferSize % 7 == 0) + { + setDriverError(BlockError); + } + else + { + if (buffer_size > 0 || m_BufferSize > 0) + { + if (m_driverErrno != BlockError) + { + setDriverError(TrembleError); + } + } + else + { + setDriverError(NotBufferError); + } + } + + m_isConnected = false; + _serial->closePort(); + } + } + } + + //重连串口 + while (isscanning() && + connect(m_port.c_str(), m_baudrate) != RESULT_OK) + { + setDriverError(NotOpenError); + delay(300); + } + //如果未连接串口或者已停止扫描，则返回 + if (!isconnected() || !isscanning()) + { + return RESULT_FAIL; + } + + //尝试启动雷达，如果成功则返回成功，否则进入下一次循环 + { + delay(50); + + if (!m_SingleChannel && m_driverErrno != BlockError) + { + device_info devinfo; + ans = getDeviceInfo(devinfo, 1000); + if (!IS_OK(ans)) + { + stopScan(); + ans = getDeviceInfo(devinfo); + } + if (!IS_OK(ans)) + { + setDriverError(DeviceNotFoundError); + continue; + } + } + + ans = startAutoScan(); + if (IS_OK(ans)) + { + if (getDriverError() == DeviceNotFoundError) + { + setDriverError(NoError); + } + return ans; + } + else + { + setDriverError(DeviceNotFoundError); + } + } + } + + return RESULT_FAIL; + } + + result_t YDlidarDriver::autoHeartBeat() + { + if (!m_isConnected) + { + return RESULT_FAIL; + } + + ScopedLocker l(_cmd_lock); + result_t ans = sendCommand(LIDAR_CMD_SCAN); + return ans; + } + + void YDlidarDriver::KeepLiveHeartBeat() + { + if (m_HeartBeat) + { + uint32_t end_ts = getms(); + + if (end_ts - m_heartbeat_ts > DEFAULT_HEART_BEAT) + { + autoHeartBeat(); + m_heartbeat_ts = end_ts; + } + } + } + + void YDlidarDriver::CheckLaserStatus() + { + if (m_InvalidNodeCount < 2) + { + if (m_driverErrno == NoError) + { + setDriverError(LaserFailureError); + } + } + else + { + if (m_driverErrno == LaserFailureError) + { + setDriverError(NoError); + } + } + + m_InvalidNodeCount = 0; + } + + // 解析扫描数据线程主函数 + int YDlidarDriver::cacheScanData() + { + node_info local_buf[128]; + size_t count = 128; + node_info local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + result_t ans = RESULT_FAIL; + memset(local_scan, 0, sizeof(local_scan)); + + int timeout_count = 0; + retryCount = 0; + m_BufferSize = 0; + m_heartbeat_ts = getms(); + bool m_last_frame_valid = false; + + m_isScanning = true; + + while (m_isScanning) + { + count = 128; + ans = waitScanData(local_buf, count, DEFAULT_TIMEOUT / 2); + if (!IS_OK(ans)) + { + if (timeout_count > DEFAULT_TIMEOUT_COUNT) + { + if (!isAutoReconnect) + { + error("Exit scanning thread!"); + m_isScanning = false; + return RESULT_FAIL; + } + else + { + if (m_last_frame_valid) + { + m_BufferSize = 0; + m_last_frame_valid = false; + } + + ans = checkAutoConnecting(IS_FAIL(ans)); + if (IS_OK(ans)) + { + timeout_count = 0; + local_scan[0].sync = NODE_UNSYNC; + } + else + { + m_isScanning = false; + return RESULT_FAIL; + } + } + } + else + { + timeout_count++; + local_scan[0].sync = NODE_UNSYNC; + + if (m_driverErrno == NoError) + setDriverError(TimeoutError); + + error("Timeout count: %d", timeout_count); + } + } + else + { + timeout_count = 0; + m_last_frame_valid = true; + + if (retryCount != 0) + { + setDriverError(NoError); + } + + retryCount = 0; + m_BufferSize = 0; + } + + for (size_t pos = 0; pos < count; ++pos) + { + if (local_buf[pos].sync & LIDAR_RESP_SYNCBIT) + { + if (local_scan[0].sync & LIDAR_RESP_SYNCBIT) + { + ScopedLocker l(_lock); + local_scan[0].delayTime = local_buf[pos].delayTime; + //TODO: 将下一圈的第一个点的采集时间作为当前圈数据的采集时间 + + memcpy(scan_node_buf, local_scan, scan_count * sizeof(node_info)); + scan_node_count = scan_count; + _dataEvent.set(); + } + + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + + if (scan_count == _countof(local_scan)) + { + scan_count -= 1; + } + } + + KeepLiveHeartBeat(); + } + + m_isScanning = false; + + return RESULT_OK; + } + + // 获取雷达模组抛出的设备信息、健康信息 + result_t YDlidarDriver::waitDevicePackage(uint32_t timeout) + { + result_t ret = RESULT_FAIL; + + uint32_t st = getms(); + uint32_t wt = 0; + while ((wt = getms() - st) <= timeout) + { + lidar_ans_header head = {0}; + ret = waitResponseHeader(&head, timeout); + if (!IS_OK(ret)) + return ret; + if (head.type != LIDAR_ANS_TYPE_DEVINFO) + continue; + if (head.size < DEVICEINFOSIZE) + continue; + + ret = waitForData(head.size, timeout); + if (!IS_OK(ret)) + return ret; + + device_info di; + getData(reinterpret_cast(&di), DEVICEINFOSIZE); + // 没有启用底板时才使用模组的雷达型号码 + if (!m_Bottom) + model = di.model; + m_HasDeviceInfo |= EPT_Module; + m_ModuleDevInfo = di; + printfDeviceInfo(di, EPT_Module); + break; + } + + flushSerial(); + return ret; + } + + void YDlidarDriver::checkBlockStatus(uint8_t currentByte) + { + switch (m_BlockRevSize) + { + case 0: + if (currentByte == LIDAR_ANS_SYNC_BYTE1) + { + m_BlockRevSize++; + } + + break; + + case 1: + if (currentByte == LIDAR_ANS_SYNC_BYTE2) + { + setDriverError(BlockError); + m_BlockRevSize = 0; + } + + break; + + default: + break; + } + } + + result_t YDlidarDriver::parseResponseHeader( + uint8_t *packageBuffer, + uint32_t timeout) + { + int recvPos = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + m_BlockRevSize = 0; + package_Sample_Num = 0; + uint8_t package_type = 0; + result_t ans = RESULT_TIMEOUT; + + while ((waitTime = getms() - startTs) <= timeout) + { + size_t remainSize = TRI_PACKHEADSIZE - recvPos; + size_t recvSize = 0; + ans = waitForData(remainSize, timeout - waitTime, &recvSize); + if (!IS_OK(ans)) + { + return ans; + } + + if (recvSize > remainSize) + recvSize = remainSize; + + ans = getData(globalRecvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) + { + uint8_t currentByte = globalRecvBuffer[pos]; + switch (recvPos) + { + case 0: + if (currentByte != PH1) + { + checkBlockStatus(currentByte); + continue; + } + break; + + case 1: + { + CheckSumCal = PH; + if (currentByte == PH2) + { + if (m_driverErrno == BlockError) + { + setDriverError(NoError); + } + } + else if (currentByte == PH1) // 防止出现连续0xAA + { + continue; + } + else if (currentByte == PH3) // 时间戳标识 + { + recvPos = 0; + size_t lastPos = pos - 1; + // 解析时间戳（共8个字节） + int remainSize = SIZE_STAMPPACKAGE - (recvSize - pos + 1); // 计算剩余应读字节数 + if (remainSize > 0) + { + size_t lastSize = recvSize; + ans = waitForData(remainSize, timeout - waitTime, &recvSize); + if (!IS_OK(ans)) + return ans; + if (recvSize > remainSize) + recvSize = remainSize; + getData(&globalRecvBuffer[lastSize], recvSize); + recvSize += lastSize; + pos = TRI_PACKHEADSIZE; + } + else + { + pos += 6; + } + // 时间戳校验和检测 + uint8_t csc = 0; // 计算校验和 + uint8_t csr = 0; // 实际校验和 + for (int i = 0; i < SIZE_STAMPPACKAGE; ++i) + { + if (i == 2) + csr = globalRecvBuffer[lastPos + i]; + else + csc ^= globalRecvBuffer[lastPos + i]; + } + if (csc != csr) + { + error("Checksum error c[0x%02X] != r[0x%02X]", csc, csr); + } + else + { + stamp_package sp; + memcpy(&sp, &globalRecvBuffer[lastPos], SIZE_STAMPPACKAGE); + stamp = uint64_t(sp.stamp) * 1000000; // 毫秒转纳秒需要×1000000 + // debug("stamp: 0x%"PRIx64" -> 0x%"PRIx64"", sp.stamp, stamp); + // 测试扫描时长 + // static uint32_t s_scanTime = 0; + // if (s_scanTime > 0) + // { + // uint32_t dt = sp.stamp - s_scanTime; + // if (dt < 44 || dt > 57) + // { + // error("单帧时长[%u]ms超出标准[%u~%u]", + // dt, 44, 57); + // } + // } + // s_scanTime = sp.stamp; + } + continue; + } + else + { + has_package_error = true; + recvPos = 0; + continue; + } + break; + } + case 2: + SampleNumlAndCTCal = currentByte; + package_type = currentByte & 0x01; // 是否是零位包标识 + if (package_type == CT_RingStart) + { + scan_frequence = (currentByte & 0xFE) >> 1; + } + break; + + case 3: + SampleNumlAndCTCal += (currentByte * 0x100); + package_Sample_Num = currentByte; + break; + + case 4: + if (currentByte & LIDAR_RESP_CHECKBIT) + { + FirstSampleAngle = currentByte; + } + else + { + has_package_error = true; + recvPos = 0; + continue; + } + break; + + case 5: + FirstSampleAngle += currentByte * 0x100; + CheckSumCal ^= FirstSampleAngle; + FirstSampleAngle = FirstSampleAngle >> 1; + break; + + case 6: + if (currentByte & LIDAR_RESP_CHECKBIT) + { + LastSampleAngle = currentByte; + } + else + { + has_package_error = true; + recvPos = 0; + continue; + } + + break; + + case 7: + LastSampleAngle = currentByte * 0x100 + LastSampleAngle; + LastSampleAngleCal = LastSampleAngle; + LastSampleAngle = LastSampleAngle >> 1; + + if (package_Sample_Num == 1) + { + IntervalSampleAngle = 0; + } + else + { + if (LastSampleAngle < FirstSampleAngle) + { + if ((FirstSampleAngle > 270 * 64) && (LastSampleAngle < 90 * 64)) + { + IntervalSampleAngle = (float)((360 * 64 + LastSampleAngle - + FirstSampleAngle) / + (( + package_Sample_Num - 1) * + 1.0)); + IntervalSampleAngle_LastPackage = IntervalSampleAngle; + } + else + { + IntervalSampleAngle = IntervalSampleAngle_LastPackage; + } + } + else + { + IntervalSampleAngle = (float)((LastSampleAngle - FirstSampleAngle) / (( + package_Sample_Num - 1) * + 1.0)); + IntervalSampleAngle_LastPackage = IntervalSampleAngle; + } + } + break; + + case 8: + CheckSum = currentByte; + break; + case 9: + CheckSum += (currentByte * 0x100); + break; + } + + packageBuffer[recvPos++] = currentByte; + } + + if (recvPos == TRI_PACKHEADSIZE) + { + ans = RESULT_OK; + break; + } + + ans = RESULT_TIMEOUT; + } + + return ans; + } + + result_t YDlidarDriver::parseResponseScanData( + uint8_t *packageBuffer, + uint32_t timeout) + { + int recvPos = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + result_t ans = RESULT_TIMEOUT; + + while ((waitTime = getms() - startTs) <= timeout) + { + size_t remainSize = package_Sample_Num * PackageSampleBytes - recvPos; + size_t recvSize = 0; + ans = waitForData(remainSize, timeout - waitTime, &recvSize); + + if (!IS_OK(ans)) + return ans; + + if (recvSize > remainSize) + recvSize = remainSize; + + getData(globalRecvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) + { + if (m_intensities && !isTOFLidar(m_LidarType)) + { + if (recvPos % 3 == 2) + { + Valu8Tou16 += globalRecvBuffer[pos] * 0x100; + CheckSumCal ^= Valu8Tou16; + } + else if (recvPos % 3 == 1) + { + Valu8Tou16 = globalRecvBuffer[pos]; + } + else + { + CheckSumCal ^= globalRecvBuffer[pos]; + } + } + else + { + if (recvPos % 2 == 1) + { + Valu8Tou16 += globalRecvBuffer[pos] * 0x100; + CheckSumCal ^= Valu8Tou16; + } + else + { + Valu8Tou16 = globalRecvBuffer[pos]; + } + } + + packageBuffer[TRI_PACKHEADSIZE + recvPos] = globalRecvBuffer[pos]; + recvPos++; + } + + if (package_Sample_Num * PackageSampleBytes == recvPos) + { + ans = RESULT_OK; + break; + } + } + + if (package_Sample_Num * PackageSampleBytes != recvPos) + { + return RESULT_FAIL; + } + + return ans; + } + + // 解析时间戳数据（云鲸雷达） + bool YDlidarDriver::parseStampData(uint32_t timeout) + { + // 如果无时间戳数据，则直接返回 + if (!hasStamp) + return true; + + // 云鲸雷达零位包后边附加8字节时间戳数据，单位纳秒 + result_t ans = RESULT_TIMEOUT; + int size = sizeof(uint64_t); + size_t ws = size; + size_t rs = 0; + ans = waitForData(ws, timeout, &rs); + if (!IS_OK(ans)) + return false; + if (rs > ws) + rs = ws; + getData(globalRecvBuffer, rs); + // 判断8个字节数据是否存在AA55，如果有则说明当前无时间戳数据 + int pos = 0; + for (int i = 0; i < rs; ++i) + { + uint8_t c = globalRecvBuffer[i]; + switch (pos) + { + case 0: + { + if (c != PH1) + continue; + break; + } + case 1: + { + if (c != PH2) + { + pos = 0; + continue; + } + hasStamp = false; + break; + } + } + } + if (hasStamp) + { + memcpy(&stamp, globalRecvBuffer, size); + // debug("SCL stamp: %llu 0x%"PRIx64"", stamp, stamp); + } + + return hasStamp; + } + + result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) + { + (*node).index = 255; + (*node).scanFreq = 0; + (*node).error = 0; + (*node).debugInfo = 0xff; + + if (nodeIndex == 0) + { + uint8_t *packageBuffer = (m_intensities) ? (isTOFLidar(m_LidarType) ? (uint8_t *)&tof_package.head : (uint8_t *)&package.head) : (uint8_t *)&packages.head; + result_t ans = parseResponseHeader(packageBuffer, timeout); + if (!IS_OK(ans)) + { + return ans; + } + ans = parseResponseScanData(packageBuffer, timeout); + if (!IS_OK(ans)) + { + return ans; + } + calcuteCheckSum(node); + calcutePackageCT(); + + // if ((ct & 0x01) == CT_RingStart) + // parseStampData(); //解析时间戳 + } + + parseNodeDebugFromBuffer(node); // 解析调试信息 + parseNodeFromeBuffer(node); // 解析点数据 + return RESULT_OK; + } + + void YDlidarDriver::calcuteCheckSum(node_info *node) + { + CheckSumCal ^= SampleNumlAndCTCal; + CheckSumCal ^= LastSampleAngleCal; + + if (CheckSumCal != CheckSum) + { + CheckSumResult = false; + has_package_error = true; + (*node).error = 1; + error("Check Sum 0x%04X != 0x%04X", CheckSumCal, CheckSum); + } + else + { + CheckSumResult = true; + } + } + + void YDlidarDriver::calcutePackageCT() + { + if (m_intensities) + { + if (isTOFLidar(m_LidarType)) + { + ct = tof_package.ct; + nowPackageNum = tof_package.count; + } + else + { + ct = package.ct; + nowPackageNum = package.count; + } + } + else + { + ct = packages.ct; + nowPackageNum = packages.count; + } + // debug("S2 pack points %u", nowPackageNum); + } + + void YDlidarDriver::parseNodeDebugFromBuffer(node_info *node) + { + if ((ct & 0x01) == CT_Normal) + { + (*node).sync = NODE_UNSYNC; + (*node).debugInfo = 0xff; + + if (!has_package_error) + { + if (nodeIndex == 0) + { + package_index++; + (*node).debugInfo = (ct >> 1); + (*node).index = package_index; + } + } + else + { + (*node).error = 1; + (*node).index = 255; + package_index = 0xff; + } + } + else + { + (*node).sync = NODE_SYNC; + package_index = 0; + + // debug("start angle %f end angle %f", + // float(FirstSampleAngle) / 64.0, + // float(LastSampleAngle) / 64.0); + + if (CheckSumResult) + { + has_package_error = false; + (*node).index = package_index; + (*node).debugInfo = (ct >> 1); + (*node).scanFreq = scan_frequence; + } + } + } + + void YDlidarDriver::parseNodeFromeBuffer(node_info *node) + { + int32_t correctAngle = 0; + (*node).qual = Node_Default_Quality; + (*node).delayTime = 0; + (*node).stamp = stamp ? stamp : getTime(); + (*node).scanFreq = scan_frequence; + (*node).is = 0; + + if (CheckSumResult) + { + if (m_intensities) // 如果带强度信息 + { + if (isTriangleLidar(m_LidarType)) + { + if (8 == m_intensityBit) + { + (*node).qual = uint16_t(package.nodes[nodeIndex].qual); + } + else + { + (*node).qual = ((uint16_t)((package.nodes[nodeIndex].dist & 0x03) << LIDAR_RESP_ANGLE_SAMPLE_SHIFT) | + (package.nodes[nodeIndex].qual)); + } + + (*node).dist = + package.nodes[nodeIndex].dist & 0xfffc; + (*node).is = package.nodes[nodeIndex].dist & 0x0003; + + // debug("%d d:%u", nodeIndex, (*node).dist); + } + else + { + (*node).qual = + tof_package.nodes[nodeIndex].qual; + (*node).dist = + tof_package.nodes[nodeIndex].dist; + } + } + else // 如果不带强度信息 + { + (*node).dist = packages.nodes[nodeIndex]; + + if (isTriangleLidar(m_LidarType)) + { + (*node).qual = ((uint16_t)(0xfc | + packages.nodes[nodeIndex] & + 0x0003)) + << LIDAR_RESP_QUALITY_SHIFT; + } + } + + if ((*node).dist != 0) + { + // debug("has angle 2nd parse %d %d", m_LidarType, model); + if (isOctaveLidar(model)) + { + correctAngle = (int32_t)(((atan(((21.8 * (155.3 - ((*node).dist / 2.0))) / 155.3) / ((*node).dist / 2.0))) * 180.0 / 3.1415) * 64.0); + } + else if (isSCLLidar(m_LidarType) || + isSCLLidar2(model)) + { + // SCL雷达角度二级解析公式 + // correctAngle = int32_t(asin(17.8 / node->dist) * 180.0 / M_PI * 64.0); + correctAngle = int32_t(atan(17.8 / (node->dist / 4.0)) * 180.0 / M_PI * 64.0); + // debug("SCL correct angle [%d]", correctAngle); + } + else if (isTriangleLidar(m_LidarType) && + !isTminiLidar(model) && // 去掉Tmini雷达的角度二级解析 + !isR3Lidar(model)) // 去掉R3雷达的角度二级解析 + { + correctAngle = (int32_t)(((atan(((21.8 * (155.3 - ((*node).dist / 4.0))) / 155.3) / ((*node).dist / 4.0))) * 180.0 / 3.1415) * 64.0); + } + else + { + //debug("no angle 2nd parse"); + } + + m_InvalidNodeCount++; + } + else + { + correctAngle = 0; + } + + float sampleAngle = IntervalSampleAngle * nodeIndex; + + if ((FirstSampleAngle + sampleAngle + + correctAngle) < 0) + { + (*node).angle = (((uint16_t)(FirstSampleAngle + sampleAngle + + correctAngle + 23040)) + << LIDAR_RESP_ANGLE_SHIFT) + + LIDAR_RESP_CHECKBIT; + } + else + { + if ((FirstSampleAngle + sampleAngle + correctAngle) > 23040) + { + (*node).angle = (((uint16_t)(FirstSampleAngle + sampleAngle + + correctAngle - 23040)) + << LIDAR_RESP_ANGLE_SHIFT) + + LIDAR_RESP_CHECKBIT; + } + else + { + (*node).angle = (((uint16_t)(FirstSampleAngle + sampleAngle + + correctAngle)) + << LIDAR_RESP_ANGLE_SHIFT) + + LIDAR_RESP_CHECKBIT; + } + } + } + else + { + (*node).sync = NODE_UNSYNC; + (*node).qual = Node_Default_Quality; + (*node).angle = LIDAR_RESP_CHECKBIT; + (*node).dist = 0; + (*node).scanFreq = 0; + } + + nodeIndex++; + + if (nodeIndex >= nowPackageNum) + { + nodeIndex = 0; + CheckSumResult = false; + } + } + + result_t YDlidarDriver::waitScanData( + node_info *nodebuffer, + size_t &count, + uint32_t timeout) + { + if (!m_isConnected) + { + count = 0; + return RESULT_FAIL; + } + + size_t recvNodeCount = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + result_t ans = RESULT_FAIL; + + while ((waitTime = getms() - startTs) <= timeout && + recvNodeCount < count) + { + node_info node; + ans = waitPackage(&node, timeout - waitTime); + if (!IS_OK(ans)) + { + count = recvNodeCount; + return ans; + } + + nodebuffer[recvNodeCount++] = node; + + // 如果是零位包点 + if (node.sync & LIDAR_RESP_SYNCBIT) + { + // 计算延时时间 + size_t size = _serial->available(); + uint64_t delayTime = 0; + if (size > TRI_PACKHEADSIZE) + { + size_t packageNum = 0; + size_t Number = 0; + size_t PackageSize = TRI_PACKMAXNODES; + packageNum = size / PackageSize; + Number = size % PackageSize; + delayTime = packageNum * (PackageSize - TRI_PACKHEADSIZE) * m_PointTime / 2; + + if (Number > TRI_PACKHEADSIZE) + { + delayTime += m_PointTime * ((Number - TRI_PACKHEADSIZE) / 2); + } + } + nodebuffer[recvNodeCount - 1].delayTime = size * trans_delay + delayTime; + + count = recvNodeCount; + CheckLaserStatus(); + return RESULT_OK; + } + + if (recvNodeCount == count) + { + return RESULT_OK; + } + } + + count = recvNodeCount; + return RESULT_FAIL; + } + + result_t YDlidarDriver::grabScanData( + node_info *nodes, + size_t &count, + uint32_t timeout) + { + switch (_dataEvent.wait(timeout)) + { + case Event::EVENT_TIMEOUT: + count = 0; + return RESULT_TIMEOUT; + case Event::EVENT_OK: + { + ScopedLocker l(_lock); + size_t size_to_copy = min(count, scan_node_count); + memcpy(nodes, scan_node_buf, size_to_copy * sizeof(node_info)); + count = size_to_copy; + scan_node_count = 0; + return RESULT_OK; + } + default: + count = 0; + return RESULT_FAIL; + } + } + + result_t YDlidarDriver::ascendScanData(node_info *nodebuffer, size_t count) + { + float inc_origin_angle = (float)360.0 / count; + int i = 0; + + for (i = 0; i < (int)count; i++) + { + if (nodebuffer[i].dist == 0) + { + continue; + } + else + { + while (i != 0) + { + i--; + float expect_angle = (nodebuffer[i + 1].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f - + inc_origin_angle; + + if (expect_angle < 0.0f) + { + expect_angle = 0.0f; + } + + uint16_t checkbit = nodebuffer[i].angle & + LIDAR_RESP_CHECKBIT; + nodebuffer[i].angle = (((uint16_t)(expect_angle * 64.0f)) << LIDAR_RESP_ANGLE_SHIFT) + checkbit; + } + + break; + } + } + + if (i == (int)count) + { + return RESULT_FAIL; + } + + for (i = (int)count - 1; i >= 0; i--) + { + if (nodebuffer[i].dist == 0) + { + continue; + } + else + { + while (i != ((int)count - 1)) + { + i++; + float expect_angle = (nodebuffer[i - 1].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f + + inc_origin_angle; + + if (expect_angle > 360.0f) + { + expect_angle -= 360.0f; + } + + uint16_t checkbit = nodebuffer[i].angle & + LIDAR_RESP_CHECKBIT; + nodebuffer[i].angle = (((uint16_t)(expect_angle * 64.0f)) << LIDAR_RESP_ANGLE_SHIFT) + checkbit; + } + + break; + } + } + + float frontAngle = (nodebuffer[0].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f; + + for (i = 1; i < (int)count; i++) + { + if (nodebuffer[i].dist == 0) + { + float expect_angle = frontAngle + i * inc_origin_angle; + + if (expect_angle > 360.0f) + { + expect_angle -= 360.0f; + } + + uint16_t checkbit = nodebuffer[i].angle & + LIDAR_RESP_CHECKBIT; + nodebuffer[i].angle = (((uint16_t)(expect_angle * 64.0f)) << LIDAR_RESP_ANGLE_SHIFT) + checkbit; + } + } + + size_t zero_pos = 0; + float pre_degree = (nodebuffer[0].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f; + + for (i = 1; i < (int)count; ++i) + { + float degree = (nodebuffer[i].angle >> + LIDAR_RESP_ANGLE_SHIFT) / + 64.0f; + + if (zero_pos == 0 && (pre_degree - degree > 180)) + { + zero_pos = i; + break; + } + + pre_degree = degree; + } + + node_info *tmpbuffer = new node_info[count]; + + for (i = (int)zero_pos; i < (int)count; i++) + { + tmpbuffer[i - zero_pos] = nodebuffer[i]; + } + + for (i = 0; i < (int)zero_pos; i++) + { + tmpbuffer[i + (int)count - zero_pos] = nodebuffer[i]; + } + + memcpy(nodebuffer, tmpbuffer, count * sizeof(node_info)); + delete[] tmpbuffer; + + return RESULT_OK; + } + + /************************************************************************/ + /* get health state of lidar */ + /************************************************************************/ + result_t YDlidarDriver::getHealth(device_health &health, uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + if (m_SingleChannel) + { + if (get_device_health_success) + { + health = this->health_; + return RESULT_OK; + } + + health.error_code = 0; + health.status = 0; + return RESULT_OK; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_HEALTH)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVHEALTH) + { + return RESULT_FAIL; + } + + if (response_header.size < sizeof(device_health)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&health), sizeof(health)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* get device info of lidar */ + /************************************************************************/ + result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) + { + result_t ans = RESULT_FAIL; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + // 单通，仅能获取到模组设备信息 + if (m_SingleChannel) + { + // 获取启动时抛出的设备信息或每帧数据中的设备信息 + if (m_HasDeviceInfo & EPT_Module) + { + info = m_ModuleDevInfo; + return RESULT_OK; + } + else + { + // 未获取到设备信息时，返回一个无效的设备信息 + info.model = YDLIDAR_S2; + return RESULT_FAIL; + } + } + // 双通，此处仅能获取到底板设备信息 + else + { + if (m_Bottom) + { + flushSerial(); + ScopedLocker l(_cmd_lock); + if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_INFO)) != RESULT_OK) + return ans; + + lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + return ans; + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + return RESULT_FAIL; + if (response_header.size < sizeof(device_info)) + return RESULT_FAIL; + if (waitForData(response_header.size, timeout) != RESULT_OK) + return RESULT_FAIL; + + getData(reinterpret_cast(&info), sizeof(info)); + model = info.model; + m_HasDeviceInfo |= EPT_Base; + m_BaseDevInfo = info; + } + return ans; + } + } + + bool YDlidarDriver::getDeviceInfoEx(device_info &info, int type) + { + if ((type & EPT_Module) && + (m_HasDeviceInfo & EPT_Module)) + { + info = m_ModuleDevInfo; + return true; + } + else if ((type & EPT_Base) && + (m_HasDeviceInfo & EPT_Base)) + { + info = m_BaseDevInfo; + return true; + } + + return false; + } + + /************************************************************************/ + /* the set to signal quality */ + /************************************************************************/ + void YDlidarDriver::setIntensities(const bool &isintensities) + { + if (m_intensities != isintensities) + { + if (globalRecvBuffer) + { + delete[] globalRecvBuffer; + globalRecvBuffer = NULL; + } + + if (isintensities && isTOFLidar(m_LidarType)) + { + globalRecvBuffer = new uint8_t[sizeof(tof_node_package)]; + } + else + { + globalRecvBuffer = new uint8_t[isintensities ? sizeof(tri_node_package2) : sizeof(tri_node_package)]; + } + } + + m_intensities = isintensities; + + if (m_intensities) + { + if (isTOFLidar(m_LidarType)) + { + PackageSampleBytes = 4; + } + else + { + PackageSampleBytes = 3; + } + } + else + { + PackageSampleBytes = 2; + } + } + + /** + * @brief 设置雷达异常自动重新连接 \n + * @param[in] enable 是否开启自动重连: + * true 开启 + * false 关闭 + */ + void YDlidarDriver::setAutoReconnect(const bool &enable) + { + isAutoReconnect = enable; + } + + void YDlidarDriver::checkTransDelay() + { + // calc stamp + trans_delay = _serial->getByteTime(); + sample_rate = getDefaultSampleRate(model).front() * 1000; + + switch (model) + { + case YDLIDAR_G4: // g4 + case YDLIDAR_G5: + case YDLIDAR_G4PRO: + case YDLIDAR_F4PRO: + case YDLIDAR_G6: // g6 + case YDLIDAR_G7: + case YDLIDAR_TG15: + case YDLIDAR_TG30: + case YDLIDAR_TG50: + if (m_sampling_rate == -1) + { + sampling_rate _rate; + _rate.rate = 0; + getSamplingRate(_rate); + m_sampling_rate = _rate.rate; + } + + sample_rate = ConvertLidarToUserSmaple(model, m_sampling_rate); + sample_rate *= 1000; + break; + + case YDLIDAR_G2C: + sample_rate = 4000; + break; + + case YDLIDAR_G1: + sample_rate = 9000; + break; + + case YDLIDAR_G4C: + sample_rate = 4000; + break; + + default: + break; + } + + m_PointTime = 1e9 / sample_rate; + } + + /************************************************************************/ + /* start to scan */ + /************************************************************************/ + result_t YDlidarDriver::startScan(bool force, uint32_t timeout) + { + result_t ret; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + if (m_isScanning) + { + return RESULT_OK; + } + + hasStamp = true; + stop(); + checkTransDelay(); + flushSerial(); + delay(30); + { + ScopedLocker l(_cmd_lock); + // 不管单双通雷达都发送启动命令 + ret = sendCommand(force ? LIDAR_CMD_FORCE_SCAN : LIDAR_CMD_SCAN); + if (!IS_OK(ret)) + return ret; + + if (!m_SingleChannel) //双通雷达 + { + //双通雷达需要等待响应 + lidar_ans_header response_header; + if ((ret = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ret; + } + if (response_header.type != LIDAR_ANS_TYPE_MEASUREMENT) + { + return RESULT_FAIL; + } + if (response_header.size < 5) + { + return RESULT_FAIL; + } + + //此处仅获取模组设备信息 + { + waitDevicePackage(1000); + } + } + + //非Tmini系列雷达才自动获取强度标识 + if (m_AutoIntensity) + { + if (!isTminiLidar(model)) + { + // 获取强度标识 + getIntensityFlag(); + } + } + + // 创建数据解析线程 + ret = createThread(); + } + + if (isSupportMotorCtrl(model)) + startMotor(); + + m_isScanning = true; + + return ret; + } + + result_t YDlidarDriver::stopScan(uint32_t timeout) + { + UNUSED(timeout); + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + ScopedLocker l(_cmd_lock); + sendCommand(LIDAR_CMD_FORCE_STOP); + delay(5); + sendCommand(LIDAR_CMD_STOP); + delay(5); + return RESULT_OK; + } + + result_t YDlidarDriver::createThread() + { + m_thread = new std::thread(&YDlidarDriver::cacheScanData, this); + if (!m_thread) + { + error("[YDLIDAR] Fail to create thread"); + return RESULT_FAIL; + } + + info("[YDLIDAR] Create thread 0x%X", m_thread->get_id()); + return RESULT_OK; + } + + result_t YDlidarDriver::startAutoScan(bool force, uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + flushSerial(); + delay(10); + { + + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(force ? LIDAR_CMD_FORCE_SCAN : LIDAR_CMD_SCAN)) != + RESULT_OK) + { + return ans; + } + + if (!m_SingleChannel) + { + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_MEASUREMENT) + { + return RESULT_FAIL; + } + + if (response_header.size < 5) + { + return RESULT_FAIL; + } + } + } + + if (isSupportMotorCtrl(model)) + { + startMotor(); + } + + return RESULT_OK; + } + + /************************************************************************/ + /* stop scan */ + /************************************************************************/ + result_t YDlidarDriver::stop() + { + disableDataGrabbing(); + stopScan(); + + if (isSupportMotorCtrl(model)) + { + stopMotor(); + } + + return RESULT_OK; + } + + /************************************************************************/ + /* reset device */ + /************************************************************************/ + result_t YDlidarDriver::reset(uint32_t timeout) + { + UNUSED(timeout); + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_RESET)) != RESULT_OK) + { + return ans; + } + + return RESULT_OK; + } + + /************************************************************************/ + /* startMotor */ + /************************************************************************/ + result_t YDlidarDriver::startMotor() + { + ScopedLocker l(_cmd_lock); + + if (m_SupportMotorDtrCtrl) + { + setDTR(); + delay(500); + } + else + { + clearDTR(); + delay(500); + } + + return RESULT_OK; + } + + /************************************************************************/ + /* stopMotor */ + /************************************************************************/ + result_t YDlidarDriver::stopMotor() + { + ScopedLocker l(_cmd_lock); + + if (m_SupportMotorDtrCtrl) + { + clearDTR(); + delay(500); + } + else + { + setDTR(); + delay(500); + } + + return RESULT_OK; + } + + /************************************************************************/ + /* get the current scan frequency of lidar */ + /************************************************************************/ + result_t YDlidarDriver::getScanFrequency( + scan_frequency &frequency, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_GET_AIMSPEED)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != sizeof(frequency)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&frequency), sizeof(frequency)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* add the scan frequency by 1Hz each time */ + /************************************************************************/ + result_t YDlidarDriver::setScanFrequencyAdd(scan_frequency &frequency, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_ADD)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != sizeof(frequency)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&frequency), sizeof(frequency)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* decrease the scan frequency by 1Hz each time */ + /************************************************************************/ + result_t YDlidarDriver::setScanFrequencyDis(scan_frequency &frequency, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_DIS)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != sizeof(frequency)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&frequency), sizeof(frequency)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* add the scan frequency by 0.1Hz each time */ + /************************************************************************/ + result_t YDlidarDriver::setScanFrequencyAddMic(scan_frequency &frequency, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_ADDMIC)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != sizeof(frequency)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&frequency), sizeof(frequency)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* decrease the scan frequency by 0.1Hz each time */ + /************************************************************************/ + result_t YDlidarDriver::setScanFrequencyDisMic(scan_frequency &frequency, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_DISMIC)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != sizeof(frequency)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&frequency), sizeof(frequency)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* get the sampling rate of lidar */ + /************************************************************************/ + result_t YDlidarDriver::getSamplingRate(sampling_rate &rate, uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_GET_SAMPLING_RATE)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != sizeof(rate)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&rate), sizeof(rate)); + m_sampling_rate = rate.rate; + } + return RESULT_OK; + } + + /************************************************************************/ + /* the set to sampling rate */ + /************************************************************************/ + result_t YDlidarDriver::setSamplingRate(sampling_rate &rate, uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_SET_SAMPLING_RATE)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != sizeof(rate)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&rate), sizeof(rate)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* the get to zero offset angle */ + /************************************************************************/ + result_t YDlidarDriver::getZeroOffsetAngle(offset_angle &angle, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_GET_OFFSET_ANGLE)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size < sizeof(offset_angle)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&angle), sizeof(angle)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* the set to heart beat */ + /************************************************************************/ + result_t YDlidarDriver::setScanHeartbeat(scan_heart_beat &beat, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_SET_HEART_BEAT)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size != 1) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&beat), sizeof(beat)); + } + return RESULT_OK; + } + + /************************************************************************/ + /* the get to zero offset angle */ + /************************************************************************/ + result_t YDlidarDriver::getAutoZeroOffsetAngle(offset_angle &angle, + uint32_t timeout) + { + result_t ans; + + if (!m_isConnected) + { + return RESULT_FAIL; + } + + flushSerial(); + { + ScopedLocker l(_cmd_lock); + + if ((ans = sendCommand(LIDAR_CMD_GET_OFFSET_ANGLE)) != RESULT_OK) + { + return ans; + } + + lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) + { + return ans; + } + + if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) + { + return RESULT_FAIL; + } + + if (response_header.size < sizeof(offset_angle)) + { + return RESULT_FAIL; + } + + if (waitForData(response_header.size, timeout) != RESULT_OK) + { + return RESULT_FAIL; + } + + getData(reinterpret_cast(&angle), sizeof(angle)); + } + return RESULT_OK; + } + + std::string YDlidarDriver::getSDKVersion() + { + return YDLIDAR_SDK_VERSION_STR; + } + + std::map YDlidarDriver::lidarPortList() + { + std::vector lst = list_ports(); + std::map ports; + + for (std::vector::iterator it = lst.begin(); it != lst.end(); it++) + { + std::string port = "ydlidar" + (*it).device_id; + ports[port] = (*it).port; + } + + return ports; + } + + result_t YDlidarDriver::parseHeader( + uint8_t &zero, + uint32_t &headPos, + uint32_t timeout) + { + int recvPos = 0; + uint32_t startTime = getms(); + uint32_t waitTime = 0; + uint8_t package_type = 0; + result_t ans = RESULT_TIMEOUT; + static uint8_t s_buff[TRI_PACKHEADSIZE * 2] = {0}; + + while ((waitTime = getms() - startTime) <= timeout) + { + size_t remainSize = TRI_PACKHEADSIZE - recvPos; + size_t recvSize = 0; + ans = waitForData(remainSize, timeout - waitTime, &recvSize); + if (!IS_OK(ans)) + return ans; + + if (recvSize > remainSize) + recvSize = remainSize; + + getData(s_buff, recvSize); + for (size_t pos = 0; pos < recvSize; ++pos) + { + uint8_t c = s_buff[pos]; + m_dataPos++; + + switch (recvPos) + { + case 0: + if (c != PH1) + continue; + headPos = m_dataPos; + break; + case 1: + if (c != PH2) + { + recvPos = 0; + continue; + } + else if (c == PH1) + { + recvPos = 1; + continue; + } + break; + + case 2: + package_type = c & 0x01; // 是否是零位包标识 + zero = (package_type == CT_RingStart); + break; + + case 3: + // package_Sample_Num = c; + break; + + case 4: + if (c & LIDAR_RESP_CHECKBIT) + { + } + else + { + recvPos = 0; + continue; + } + break; + + case 5: + break; + + case 6: + if (c & LIDAR_RESP_CHECKBIT) + { + } + else + { + recvPos = 0; + continue; + } + break; + + case 7: + case 8: + case 9: + default: + break; + } + + recvPos++; + } + + if (recvPos == TRI_PACKHEADSIZE) + { + ans = RESULT_OK; + break; + } + + ans = RESULT_TIMEOUT; + } + + return ans; + } + +#define ZERO_OFFSET12 12 // 零位包数据长度12（不带光强） +#define ZERO_OFFSET13 13 // 零位包数据长度13（带光强） + result_t YDlidarDriver::getIntensityFlag() + { + // 只针对三角雷达 + if (!isTriangleLidar(m_LidarType)) + return RESULT_OK; + + info("Start to getting intensity flag"); + + m_dataPos = 0; + uint32_t lastOffset = 0; + // 遍历5圈，如果5圈结果一致则认为准确 + int i = 5; + while (i-- > 0) + { + uint8_t zero = 0; // 零位包标记 + uint32_t headPos = 0; // 包头位置 + uint32_t offset = 0; // 当前圈零位包长度 + uint8_t lastZero = 0; // 上一包是否是零位包标记 + uint32_t lastPos = 0; // 上一包包头位置 + while (IS_OK(parseHeader(zero, headPos, 500))) + { + if (zero) + { + lastZero = 1; + } + else + { + if (lastZero) + { + lastZero = 0; + + offset = headPos - lastPos; + if (offset != ZERO_OFFSET12 && + offset != ZERO_OFFSET13) + break; + + if (lastOffset && + lastOffset != offset) + { + warn("Fail to getting intensity"); + return RESULT_FAIL; + } + + lastOffset = offset; + break; + } + } + + lastPos = headPos; + } + } + + if (lastOffset) + { + if (lastOffset == ZERO_OFFSET12) + { + setIntensities(false); + } + else if (lastOffset == ZERO_OFFSET13) + { + setIntensities(true); + m_intensityBit = 8; + } + info("Auto set intensity %d", m_intensities); + } + + info("[YDLIDAR] End to getting intensity flag"); + + return RESULT_OK; + } + +bool YDlidarDriver::getPitchAngle(float& pitch) +{ + if (!m_isConnected || m_SingleChannel) + return false; + + result_t ret = RESULT_OK; + ScopedLocker l(_cmd_lock); + ret = sendCommand(LIDAR_CMD_GETPITCH); + if (!IS_OK(ret)) + return false; + uint32_t timeout = TIMEOUT_300; + uint32_t st = getms(); + uint32_t wt = 0; + while ((wt = getms() - st) <= timeout) + { + lidar_ans_header head = {0}; + ret = waitResponseHeader(&head, timeout); + if (!IS_OK(ret)) + return ret; + if (head.type != LIDAR_ANS_TYPE_PITCH) + continue; + if (head.size < 4) //数据大小为4字节 + return false; + ret = waitForData(head.size, timeout); + if (!IS_OK(ret)) + return ret; + + int32_t p = 0; + getData(reinterpret_cast(&p), 4); + pitch = p / 100.0f; //缩小100倍 + return true; + } + + return false; +} + +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.h new file mode 100644 index 0000000..29f966d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/YDlidarDriver.h @@ -0,0 +1,654 @@ +// +// 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. +// +/** @page YDlidarDriver + * YDlidarDriver API + +
Library YDlidarDriver +
File ydlidar_driver.h +
Author Tony [code at ydlidar com] +
Source https://github.com/ydlidar/YDLidar-SDK +
Version 1.0.0 +
+ This YDlidarDriver support [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) and [TYPE_TOF](\ref LidarTypeID::TYPE_TOF) LiDAR + +* @copyright Copyright (c) 2018-2020 EAIBOT + Jump to the @link ::ydlidar::YDlidarDriver @endlink interface documentation. +*/ + +#ifndef YDLIDAR_DRIVER_H +#define YDLIDAR_DRIVER_H +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if !defined(__cplusplus) +#ifndef __cplusplus +#error "The YDLIDAR SDK requires a C++ compiler to be built" +#endif +#endif + + +using namespace std; + +namespace ydlidar { +using namespace core; +using namespace core::common; + +/*! +* Class that provides a lidar interface. +*/ +class YDlidarDriver : public DriverInterface { + public: + /*! + * A constructor. + * A more elaborate description of the constructor. + */ + explicit YDlidarDriver(uint8_t type = YDLIDAR_TYPE_SERIAL); + + /*! + * A destructor. + * A more elaborate description of the destructor. + */ + virtual ~YDlidarDriver(); + + /** + * @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); + + /** + * @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); + + + /*! + * @brief Disconnect the LiDAR. + */ + virtual void disconnect(); + + /** + * @brief Get SDK Version \n + * static function + * @return Version + */ + virtual std::string getSDKVersion(); + + /** + * @brief lidarPortList Get Lidar Port lists + * @return online lidars + */ + static std::map lidarPortList(); + + + /** + * @brief Is the Lidar in the scan \n + * @return scanning status + * @retval true scanning + * @retval false non-scanning + */ + virtual bool isscanning() const; + + /** + * @brief Is it connected to the lidar \n + * @return connection status + * @retval true connected + * @retval false Non-connected + */ + virtual bool isconnected() const; + + /** + * @brief Is there intensity \n + * @param[in] isintensities intentsity + * true intensity + * false no intensity + */ + virtual void setIntensities(const bool &isintensities); + + /** + * @brief whether to support hot plug \n + * @param[in] enable hot plug : + * true support + * false no support + */ + virtual void setAutoReconnect(const bool &enable); + + /** + * @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); + + /** + * @brief get Device information \n + * @param[in] info 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 &info, + uint32_t timeout = DEFAULT_TIMEOUT); + + //获取设备信息 + virtual bool getDeviceInfoEx(device_info &info, int type=EPT_Module); + + /** + * @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) ; + + /** + * @brief turn off scanning \n + * @return result status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + virtual result_t stop(); + + /** + * @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) ; + + /** + * @brief Normalized angle \n + * Normalize the angel between 0 and 360 + * @param[in] nodebuffer Laser data + * @param[in] count one circle of laser points + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Before the normalization, you must use the ::grabScanData function to get the laser data successfully. + */ + result_t ascendScanData(node_info *nodebuffer, size_t count); + + /** + * @brief reset lidar \n + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + result_t reset(uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief start motor \n + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + result_t startMotor(); + + /** + * @brief stop motor \n + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + result_t stopMotor(); + + /** + * @brief Get lidar scan frequency \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 getScanFrequency(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + /** + * @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); + + protected: + + /** + * @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 scanning state,perform currect operation. + */ + result_t getAutoZeroOffsetAngle(offset_angle &angle, + uint32_t timeout = DEFAULT_TIMEOUT); + /** + * @brief Data parsing thread \n + * @note Before you create a dta parsing thread, you must use the ::startScan function to start the lidar scan successfully. + */ + result_t createThread(); + + /** + * @brief Automatically reconnect the lidar \n + * @param[in] force scan model + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Lidar abnormality automatically reconnects. + */ + result_t startAutoScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT) ; + + /*! + * @brief stop Scanning state + * @param timeout timeout + * @return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + result_t stopScan(uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief waiting device information + * @param timeout timeout + * @return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + result_t waitDevicePackage(uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief parseResponseHeader + * @param packageBuffer + * @param timeout + * @return + */ + result_t parseResponseHeader(uint8_t *packageBuffer, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief parseResponseScanData + * @param packageBuffer + * @param timeout + * @return + */ + result_t parseResponseScanData(uint8_t *packageBuffer, + uint32_t timeout = DEFAULT_TIMEOUT); + + //解析时间戳数据（云鲸雷达） + bool parseStampData(uint32_t timeout = DEFAULT_TIMEOUT / 10); + + /** + * @brief Unpacking \n + * @param[in] node lidar point information + * @param[in] timeout timeout + */ + result_t waitPackage(node_info *node, uint32_t timeout = DEFAULT_TIMEOUT); + /** + * @brief get unpacked data \n + * @param[in] nodebuffer laser node + * @param[in] count lidar points size + * @param[in] timeout timeout + * @return result status + * @retval RESULT_OK success + * @retval RESULT_TIMEOUT timeout + * @retval RESULT_FAILE failed + */ + result_t waitScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief data parsing thread \n + */ + int cacheScanData(); + + /** + * @brief send data to lidar \n + * @param[in] cmd command code + * @param[in] payload payload + * @param[in] payloadsize payloadsize + * @return result status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + result_t sendCommand(uint8_t cmd, const void *payload = NULL, + size_t payloadsize = 0); + + /** + * @brief waiting for package header \n + * @param[in] header package header + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_TIMEOUT timeout + * @retval RESULT_FAILE failed + * @note when timeout = -1, it will block... + */ + result_t waitResponseHeader(lidar_ans_header *header, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief Waiting for the specified size data from the lidar \n + * @param[in] data_count wait max data size + * @param[in] timeout timeout + * @param[in] returned_size really data size + * @return return status + * @retval RESULT_OK success + * @retval RESULT_TIMEOUT wait timeout + * @retval RESULT_FAILE failed + * @note when timeout = -1, it will block... + */ + result_t waitForData(size_t data_count, uint32_t timeout = DEFAULT_TIMEOUT, + size_t *returned_size = NULL); + + /** + * @brief get data from serial \n + * @param[in] data data + * @param[in] size date size + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + result_t getData(uint8_t *data, size_t size); + + /** + * @brief send data to serial \n + * @param[in] data data + * @param[in] size data size + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + result_t sendData(const uint8_t *data, size_t size); + + + /*! + * @brief checkTransDelay + */ + void checkTransDelay(); + + /** + * @brief disable Data scan channel \n + */ + void disableDataGrabbing(); + + /*! + * @brief set DTR \n + */ + void setDTR(); + + /*! + * @brief clear DTR \n + */ + void clearDTR(); + + /*! + * @brief flushSerial + */ + void flushSerial(); + + /*! + * @brief checkAutoConnecting + */ + result_t checkAutoConnecting(bool serialError = true); + + /** + * @brief autoHeartBeat + * @return + */ + result_t autoHeartBeat(); + + /** + * @brief KeepLiveHeartBeat + */ + void KeepLiveHeartBeat(); + + /** + * @brief CheckLaserStatus + */ + void CheckLaserStatus(); + + /** + * @brief checkBlockStatus + */ + void checkBlockStatus(uint8_t currentByte); + + /** + * @brief calcuteCheckSum + * @param node + */ + void calcuteCheckSum(node_info *node); + /** + * @brief calcutePackageCT + */ + void calcutePackageCT(); + /** + * @brief parseNodeDebugFromBuffer + */ + void parseNodeDebugFromBuffer(node_info *node); + + /** + * @brief parseNodeFromeBuffer + */ + void parseNodeFromeBuffer(node_info *node); + + //解析点云数据包头 + result_t parseHeader( + uint8_t &zero, + uint32_t &headPos, + uint32_t timeout = DEFAULT_TIMEOUT / 2); + //解析点云数据并判断带不带强度信息 + virtual result_t getIntensityFlag(); + //获取俯仰角值 + virtual bool getPitchAngle(float& pitch); + + private: + /// package sample bytes + int PackageSampleBytes; + /// serial port + ChannelDevice *_serial; + /// sampling inteval + uint32_t trans_delay; + /// sampling rate + int m_sampling_rate; + /// LiDAR model + int model; + int sample_rate; + + /// has intensity protocol package + tri_node_package2 package; + /// TOF Lidar has intensity protocol package + tof_node_package tof_package; + /// non-intensity protocol package + tri_node_package packages; + + float IntervalSampleAngle; + float IntervalSampleAngle_LastPackage; + /// First sample angle + uint16_t FirstSampleAngle; + /// last sample angle + uint16_t LastSampleAngle; + /// checksum + uint16_t CheckSum; + /// scan frequency + uint8_t scan_frequence; + + uint16_t CheckSumCal; + uint16_t SampleNumlAndCTCal; + uint16_t LastSampleAngleCal; + bool CheckSumResult; + uint16_t Valu8Tou16; + uint8_t ct; + uint8_t nowPackageNum; + uint8_t package_Sample_Num; + + uint8_t *globalRecvBuffer; + bool has_device_header; + uint8_t last_device_byte; + int asyncRecvPos; + uint16_t async_size; + + device_health health_; + lidar_ans_header header_; + uint8_t *headerBuffer; + uint8_t *healthBuffer; + bool get_device_health_success; + + int package_index; + bool has_package_error; + uint32_t m_heartbeat_ts; + uint8_t m_BlockRevSize; + + uint32_t m_dataPos = 0; //记录当前解析到的数据的位置（解析是否带强度信息专用） + uint64_t stamp = 0; //时间戳 + bool hasStamp = true; //是否有时间戳数据 +}; + +}// namespace ydlidar + +#endif // YDLIDAR_DRIVER_H diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/CMakeLists.txt b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/CMakeLists.txt new file mode 100644 index 0000000..a224b85 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/CMakeLists.txt @@ -0,0 +1,4 @@ +aux_include_directory(. HDRS) +aux_src_directory(. SRCS) +add_to_ydlidar_headers(${HDRS}) +add_to_ydlidar_sources(${SRCS}) diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/FilterInterface.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/FilterInterface.h new file mode 100644 index 0000000..e27d3c1 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/FilterInterface.h @@ -0,0 +1,35 @@ +#ifndef FILTERINTERFACE_H +#define FILTERINTERFACE_H +#include +#include "core/common/ydlidar_protocol.h" + + +class FilterInterface +{ +public: + FilterInterface() {} + virtual ~FilterInterface() {} + + virtual void filter(const LaserScan &in, + int lidarType, + int version, + LaserScan &out) = 0; + virtual std::string name() const { + return m_name; + } + virtual std::string version() const { + return "V1.0"; + } + virtual void setName(const std::string &name) { + m_name = name; + } + virtual void setStrategy(int value) { + m_strategy = value; + } + +protected: + int m_strategy = 0; + std::string m_name; +}; + +#endif // FILTERINTERFACE_H diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.cpp new file mode 100644 index 0000000..bce267d --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.cpp @@ -0,0 +1,601 @@ +#include +#include "NoiseFilter.h" +#include "math/angles.h" + +NoiseFilter::NoiseFilter() + : minIncline(0.11), + maxIncline(3.0), + nonMaskedNeighbours(4), + maskedNeighbours(2), + m_Monotonous(false), + maskedFilter(true) +{ + m_name = "NoiseFilter"; + setStrategy(FS_Normal); +} + +NoiseFilter::~NoiseFilter() +{ +} + +//计算两点连线和原点的夹角（弧度） +double NoiseFilter::calcInclineAngle( + double reading1, + double reading2, + double angleBetweenReadings) const +{ + return atan2(sin(angleBetweenReadings) * reading2, + reading1 - (cos(angleBetweenReadings) * reading2)); +} + +//计算两点的倾斜角（弧度） +double NoiseFilter::calcTargetAngle( + double reading1, + double angle1, + double reading2, + double angle2) +{ + double reading1_x = reading1 * cos(angle1); + double reading1_y = reading1 * sin(angle1); + double reading2_x = reading2 * cos(angle2); + double reading2_y = reading2 * sin(angle2); + double dx = reading2_x - reading1_x; + double dy = reading2_y - reading1_y; + return atan2(dy, dx); +} + +double NoiseFilter::calcTargetOffset( + double reading1, + double angle1, + double reading2, + double angle2) +{ + double target_angle = calcTargetAngle(reading1, angle1, reading2, angle2); + double cos_inv_angle = cos(-target_angle); + double sin_inv_angle = sin(-target_angle); + double reading2_x = reading2 * cos(angle2); + double reading2_y = reading2 * sin(angle2); + double offset = reading2_x * sin_inv_angle + reading2_y * cos_inv_angle; + return offset; +} + +bool NoiseFilter::isRangeValid(const LaserConfig &config, + double reading) const { + if (reading >= config.min_range && reading <= config.max_range) { + return true; + } + + return false; +} + +bool NoiseFilter::isIncreasing(double value) const { + if (value > 0) { + return true; + } + + return false; +} + +void NoiseFilter::filter_noise( + const LaserScan &in, + int /*lidarType*/, + int /*version*/, + LaserScan &out) +{ + //range is empty + if (in.points.empty()) { + out = in; + return; + } + + std::vector maskedPoints; + double lastRange = in.points[0].range; + double lastAngle = in.points[0].angle; + double lastInclineRange = lastRange; + const int nrPoints = in.points.size(); + maskedPoints.resize(nrPoints, false); + double lastIncline = 0; + bool hasFirst = false; + + //copy attributes to filtered scan + out = in; + int pointCount = 0; + double lastOffset = 0; + double lastDistance = lastRange; + double preAngle = lastAngle; + double lastDiff = 0; + bool isIncrease = false; + int minIndex = 0; + double minIndexDistance = 0; + double maxDistance = 0; + bool isNoise = false; + float filter_offset = 0.05; + + for (int i = 0; i < nrPoints; i++) { + double current_range = in.points[i].range;//current lidar distance + double current_angle = in.points[i].angle;//current lidar angle + + if (isRangeValid(in.config, current_range)) { + + double offset; + + if (isRangeValid(in.config, lastRange)) { + offset = calcTargetOffset(lastRange, lastAngle, current_range, + current_angle);//calculate offset distance + } else { + offset = calcTargetOffset(lastDistance, preAngle, current_range, + current_angle);//calculate offset distance + } + + double Diff = current_range - lastDistance;//distance difference + + if (fabs(Diff) > lastDistance * 0.2 && fabs(offset) < 0.2 && + isRangeValid(in.config, lastDistance)) { + isNoise = true; + filter_offset = fabs(offset) + 0.05; + maxDistance = current_range; + maskedPoints[i] = true; + } + + if (isNoise && fabs(offset) > filter_offset) { + isNoise = false; + } + + if (isNoise) { + if (pointCount == 0) { + minIndex = i; + minIndexDistance = current_range; + } + + if (current_range > maxDistance) { + maxDistance = current_range; + } + + pointCount++; + + // if (pointCount >= 2) { + // for (int j = minIndex - nonMaskedNeighbours; j <= i + nonMaskedNeighbours; + // j++) { + // if (j >= 0 && j < nrPoints) { + // if (j < minIndex || j > i) { + // double offset = calculateTargetOffset(in.points[j].range, in.points[j].angle, + // current_range, + // current_angle);//calculate offset distance + + // if (in.points[j].range > maxDistance) { + // maxDistance = in.points[j].range; + // } + + // if (offset < filter_offset && maxDistance > filter_offset && + // maxDistance > 0.2) { + // maskedPoints[j] = true; + // } + // } else { + // maskedPoints[i] = true; + // } + // } + // } + // } + + } else { + if (pointCount >= 2) { + bool isincre = (in.points[minIndex].range - lastDistance) < 0; + + for (int j = minIndex - nonMaskedNeighbours; j <= i + nonMaskedNeighbours; + j++) { + if (j >= 0 && j < nrPoints) { + double offset = calcTargetOffset(in.points[j].range, in.points[j].angle, + lastDistance, + preAngle);//calculate offset distance + + if (in.points[j].range > maxDistance) { + maxDistance = in.points[j].range; + } + + if (offset < filter_offset && maxDistance > filter_offset && + maxDistance > 0.2) { + maskedPoints[j] = true; + } + } + } + } + + pointCount = 0; + minIndex = 0; + maxDistance = 0.0; + } + + lastOffset = offset;//last offset + lastDiff = Diff;//last distance difference + lastDistance = current_range;//last distance + preAngle = current_angle; + } + + lastAngle = current_angle;//last angle + lastRange = current_range;//last range + } + + //mark all masked points as invalid in scan + for (unsigned int i = 0; i < in.points.size(); i++) { + if (maskedPoints[i]) { + //as we don't have a better error this is an other range error for now + out.points[i].range = 0.0; + } + } +} + +void NoiseFilter::filter( + const LaserScan &in, + int lidarType, + int version, + LaserScan &out) +{ + if (FS_Normal == m_strategy) + { + filter_noise(in, lidarType, version, out); + } + else if (FS_Tail == m_strategy) + { + filter_tail(in, lidarType, version, out); + } + else + { + filter_tail2(in, lidarType, version, out); + } +} + +void NoiseFilter::filter_tail( + const LaserScan &in, + int /*lidarType*/, + int /*version*/, + LaserScan &out) +{ +// printf("%s\n", __FUNCTION__); + + //假设激光的原点是O，对于任何两个点P1和P2，则形成角∠OP1P2， + //如果该角度小于最小阈值角度（min_angle）或大于最大阈值角度（max_angle）， + //我们将该点及其附近符合条件的点移除。 + + //range is empty + if (in.points.empty()) { + out = in; + return; + } + + std::vector maskedPoints; + double lastRange = in.points[0].range; + double lastAngle = in.points[0].angle; + double lastInclineRange = lastRange; + const int size = in.points.size(); + maskedPoints.resize(size, false); + double lastIncline = 0; + bool hasFirst = false; + + //copy attributes to filtered scan + out = in; + double lastDistance = 0; + int max_skip_step = 5 * maskedNeighbours; + + if (max_skip_step > 7) { + max_skip_step = 7; + } + + std::vector m_block_vct; + FilterBlock m_block; + bool isNextBlock = true; + int inValidPointCount = 0; + + for (int i = 0; i < size; i++) + { + double range = in.points[i].range;//current lidar distance + double angle = in.points[i].angle;//current lidar angle + + if (isRangeValid(in.config, range) + /*&& isRangeValid(in.config, lastDistance)*/) + { + if (maskedFilter && isRangeValid(in.config, lastDistance)) + { + const double incline = calcInclineAngle( + in.points[i].range, + lastDistance, + angle - lastAngle); + + if (!hasFirst) { + hasFirst = true; + lastIncline = incline; + } + + bool isValid = false; + + //this is a filter for false readings that do occur if one scannes over edgeds of objects + //如果计算的夹角超出规定的范围 + if (incline < minIncline || incline > maxIncline) + { + //mask neighbour points + for (int j = -maskedNeighbours; j < maskedNeighbours; j++) + { + if ((int(i) + j < 0) + || ((int(i) + j) > size)) { + continue; + } + if (i + j - 1 < 0) { + continue; + } + + //如果当前点相邻N点中有偏移量较大的点则认为是噪点 + double offset = calcTargetOffset(in.points[i + j - 1].range, + in.points[i + j - 1].angle, + in.points[i + j].range, + in.points[i + j].angle); //calculate offset distance + if (offset < 0.2) { + maskedPoints[i + j] = true; + isValid = true; + } + } + + if (isValid) { + if (isNextBlock) { + isNextBlock = false; + m_block.start_index = i; + } + + m_block.end_index = i; + inValidPointCount = 0; + } else { + inValidPointCount++; + } + } + else + { + inValidPointCount++; + } + + //如果上一个夹角和当前夹角差值过大则认为是噪点 + if (fabs(lastIncline - incline) > maxIncline - minIncline) { + maskedPoints[i] = true; + } + + lastInclineRange = range; + lastIncline = incline; + } + + lastDistance = range;//last distance + } + + if (inValidPointCount > max_skip_step) { + if (!isNextBlock) { + m_block_vct.push_back(m_block); + isNextBlock = true; + } + } + + lastAngle = angle;//last angle + lastRange = range;//last range + } + + /*for (int i = 0; i < m_block_vct.size(); i++) { + if (m_block_vct[i].end_index - m_block_vct[i].start_index > 20) { + int first_index = m_block_vct[i].start_index; + + if (i > 0 && + (m_block_vct[i - 1].end_index - m_block_vct[i - 1].start_index) > 2 * + maskedNeighbours) { + if (m_block_vct[i].start_index - m_block_vct[i - 1].end_index <= 2 * + (max_skip_step + 1)) { + first_index = m_block_vct[i - 1].start_index; + } + } + + if (in.points[first_index].range > 0) { + for (int k = first_index; k >= 0; k--) { + if (in.points[k].range > in.points[first_index].range) { + first_index = k; + break; + } + } + } + + for (int j = first_index - maskedNeighbours; + j < m_block_vct[i].end_index + maskedNeighbours; j++) { + if (j >= 0 && j < nrPoints) { + maskedPoints[j] = true; + } + } + } else { + if (m_block_vct[i].end_index - m_block_vct[i].start_index > maskedNeighbours) { + int first_index = m_block_vct[i].start_index - maskedNeighbours; + + for (int j = first_index; + j < m_block_vct[i].end_index + maskedNeighbours; j++) { + if (j >= 0 && j < nrPoints) { + maskedPoints[j] = true; + } + } + } + } + }*/ + + //mark all masked points as invalid in scan + for (unsigned int i = 0; i < in.points.size(); i++) { + if (maskedPoints[i]) { + //as we don't have a better error this is an other range error for now + out.points[i].range = 0.0; + } + } + + // for (int i = 0; i < m_block_vct.size(); i++) { + //// if (m_block_vct[i].end_index - m_block_vct[i].start_index > 3 * + //// maskedNeighbours) { + + // printf("block[%d]: %d~%d[%f-%f]\n", i, m_block_vct[i].start_index, + // m_block_vct[i].end_index, + // in.points[m_block_vct[i].start_index].angle * 180 / M_PI, + // in.points[m_block_vct[i].end_index].angle * 180 / M_PI); + //// } + // } +} + +void NoiseFilter::filter_tail2( + const LaserScan &in, + int /*lidarType*/, + int /*version*/, + LaserScan &out) +{ + // LOG_DEBUG("[{}] 点数[{}]", + // m_Name.toStdString().c_str(), + // in.points.size()); + + out = in; + + if (in.points.empty()) { + return; + } + + //1、找出连续（至少3个）点倾斜角朝向原点（极点）的点序列 + //2、判断该点序列首尾点组成的角度范围是否在光斑对应角度范围内 + //3、判断该点序列的强度信息是否满足约定条件（未找到规律，暂未使用） + //4、去掉该点序列的首尾点（首尾点是正常的） + + std::vector noises; //是否为噪点的标记 + size_t size = in.points.size(); //一圈点数 + size_t lastIndex = 0; //上一个有效点的索引位置 + LaserPoint lastP; //上一个点信息 + float lastIncline = .0; //上一个倾斜角 + float lastAngle = 90.0; //上一个夹角 + size_t pos = 0; //标记拖尾起始点下标位置 + // bool hasNoise = false; //是否需要处理噪点的标志 + size_t sizeEx = size + (size * 2 / 100 + 1); //将遍历范围扩大到原数组的102%以便处理首尾部分的点 + + noises.resize(size, false); + + //主循环函数 + for (size_t i = 0; i < sizeEx; ++i) + { + const LaserPoint& p = in.points.at(i % size); + + if (!isRangeValid(in.config, p.range)) + { + continue; + } + + if (i != 0) + { + //计算两点连线、两点中间点到原点连线的倾斜角（弧度值） + float incline2 = calcTargetAngle( + lastP.range, + lastP.angle, + p.range, + p.angle); + float incline3 = calcTargetAngle( + (lastP.range + p.range) / 2.0f, + (lastP.angle + p.angle) / 2.0f, + 0.0f, + 0.0f); + //转角度值 + incline2 = ydlidar::core::math::to_degrees(incline2); + incline3 = ydlidar::core::math::to_degrees(incline3); + + float incline = incline2; + + //计算两点连线和两点中间点到原点连线的夹角 + float angle = fabs(incline2 - incline3); + if (angle > 180.0f) + angle = 360.0f - angle; + if (angle > 90.0f) + angle = 180.0f - angle; + + // LOG_DEBUG("i:{} l1:{} l2:{} ia:{} range:{} angle:{} intensity:{}", + // i, incline2, + // incline3, + // angle, + // p.range, + // qRadiansToDegrees(p.angle), + // p.intensity); + + //如果倾斜角变化很小则认为是一条直线上的 + if (fabs(incline - lastIncline) < maxInclineAngle) + { + //如果上一个夹角不满足要求 + if (fabs(lastAngle) >= maxIncludeAngle) + { + pos = 0; + } + //TODO: 需要考虑是否是最后一个点 + } + else + { + if (fabs(lastAngle) < maxIncludeAngle) //判断上一个夹角是否满足要求 + { + //判断点的个数是否超过2个，超过2个才可能是拖尾噪点 + if (0 != pos && i - pos >= MIN_NOISEPOINT_COUNT) + { + //统计从位置pos到i的有效点数 + size_t validCount = 0; + for (size_t j=pos; j<=i; ++j) + { + if (isRangeValid(in.config, in.points[i % size].range)) + validCount += 1; + } + if (validCount >= MIN_NOISEPOINT_COUNT) + { + for (size_t j=pos; j<=i; ++j) + { + noises[j % size] = true; + // LOG_DEBUG("noise i:{}", j % size); + } + } + } + pos = 0; + } + if (0 == pos && fabs(angle) < maxIncludeAngle) //判断当前夹角是否满足要求 + { + //疑似拖尾点，标记 + pos = lastIndex; + } + else + { + pos = 0; + } + } + + lastIncline = incline; + lastAngle = angle; + } + + lastIndex = i; + lastP = p; + } + + //处理被标记的点 + size_t noiseCount = 0; + for (size_t i = 0; i < size; ++i) + { + if (noises[i]) + { + out.points[i].range = 0.0f; + noiseCount ++; + } + } + printf("Noise count %lu\n", noiseCount); +} + +void NoiseFilter::setStrategy(int value) +{ + FilterInterface::setStrategy(value); + + if (m_strategy == FS_TailWeek) + { + maxIncludeAngle /= 3.0f; + maxInclineAngle /= 3.0f; + } + else if (m_strategy == FS_TailStrong2) + { + maxIncludeAngle *= 1.5f; + maxInclineAngle *= 1.5f; + } +} + +std::string NoiseFilter::version() const +{ + return "1.0.1"; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.h new file mode 100644 index 0000000..bfdbe47 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/NoiseFilter.h @@ -0,0 +1,107 @@ +#ifndef NOISEFILTER_H +#define NOISEFILTER_H +#include "FilterInterface.h" + +#define MAX_INCLUDE_ANGLE 12.0f //最大夹角 +#define MAX_INCLINE_ANGLE 7.0f //最大倾斜角 +#define MIN_NOISEPOINT_COUNT 2 //最小噪点数 + + +struct FilterBlock +{ + int start_index; + int end_index; +}; + +class YDLIDAR_API NoiseFilter : public FilterInterface +{ +public: + enum FilterStrategy + { + FS_Normal, //滤噪 + FS_Tail, //旧拖尾滤波 + FS_TailStrong, //拖尾滤波 + FS_TailWeek, //拖尾滤波 + FS_TailStrong2, //拖尾滤波 + }; +public: + NoiseFilter(); + ~NoiseFilter() override; + void filter(const LaserScan &in, + int lidarType, + int version, + LaserScan &out) override; + + std::string version() const; + void setStrategy(int value) override; + +protected: + void filter_noise(const LaserScan &in, + int lidarType, + int version, + LaserScan &out); + //过滤拖尾方式1 + void filter_tail(const LaserScan &in, + int lidarType, + int version, + LaserScan &out); + //过滤拖尾方式2 + void filter_tail2(const LaserScan &in, + int lidarType, + int version, + LaserScan &out); + + double calcInclineAngle(double reading1, double reading2, + double angleBetweenReadings) const; + /** + * @brief getTargtAngle + * @param reading1 + * @param angle1 + * @param reading2 + * @param angle2 + * @return + */ + double calcTargetAngle(double reading1, double angle1, + double reading2, double angle2); + + /** + * @brief calculateTargetOffset + * @param reading1 + * @param angle1 + * @param reading2 + * @param angle2 + * @return + */ + double calcTargetOffset(double reading1, double angle1, + double reading2, double angle2); + + /** + * @brief isRangeValid + * @param reading + * @return + */ + bool isRangeValid(const LaserConfig &config, double reading) const; + + /** + * @brief isIncreasing + * @param value + * @return + */ + bool isIncreasing(double value) const; + + /** + * Defines how many readings next to an invalid reading get marked as invalid + * */ + +protected: + double minIncline, maxIncline; + int nonMaskedNeighbours; + int maskedNeighbours; + bool m_Monotonous; + bool maskedFilter; + + float maxIncludeAngle = MAX_INCLUDE_ANGLE; + float maxInclineAngle = MAX_INCLINE_ANGLE; +}; + +#endif // NOISEFILTER_H diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.cpp new file mode 100644 index 0000000..f8110ac --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.cpp @@ -0,0 +1,214 @@ +#include +#include +#include "core/math/angles.h" +#include "StrongLightFilter.h" + +#define MIN_VALUE 1e-8 +#define IS_ZERO(v) (abs(v) < MIN_VALUE) +#define IS_EQUAL(v1, v2) IS_ZERO(v1 - v2) + +StrongLightFilter::StrongLightFilter() +{ +} + +StrongLightFilter::~StrongLightFilter() +{ +} + +void StrongLightFilter::filter( + const LaserScan &in, + int lidarType, + int version, + LaserScan &out) +{ + int size = in.points.size(); // 点数 + // 按角度排序 + std::map ais; + for (int i = 0; i < size; ++i) + { + const LaserPoint &p = in.points.at(i); + if (IS_ZERO(p.range)) + continue; + ais[p.angle] = i; + } + // printf("按角度排序从[%d]点到[%d]点\n", size, ais.size()); + + size = ais.size(); // 更新点数（过滤无效点后的点数） + out = in; + out.points.resize(size); // 更新点数 + std::map::iterator it; + int i = 0; + for (it = ais.begin(); it != ais.end(); ++it) + { + out.points[i++] = in.points.at(it->second); + } + // 初始化变量 + std::vector noises(size, false); // 是否为噪点的标记 + // 将遍历范围扩大到原数组的104%以便处理首尾部分的点 + int sizeEx = int(size * 1.04); + int startI = -1; // 标记拖尾起始点下标位置 + LaserPoint lastP; // 上一个点信息 + + // 主循环函数 + for (int i = 0; i < sizeEx; ++i) + { + const LaserPoint &p = out.points.at(i % size); + if (i != 0) + { + + Point p1 = Point::polar2Angular(Point(p.angle, p.range)); + Point p2 = Point::polar2Angular(Point(lastP.angle, lastP.range)); + Point p3 = Point(0, 0); //原点 + Point p4 = Point((p1.x + p2.x) / 2.0f, (p1.y + p2.y) / 2.0f); //两点中点 + // 计算两点连线到原点的距离（直角坐标系） + float d = Point::calcDist(p3, p1, p2); + //计算两线段组成直线的夹角（直角坐标系） + float a = Point::calcAngle(p1, p2, p3, p4); + + // printf("点[%d]距离[%.03f]\n", i % size, d); + // 如果当前距离小于标准，且角度小于标准，则认为是拖尾点 + if (d < maxDist && a < maxAngle) + { + // 如果起始点无效则标记 + if (-1 == startI) + startI = i; + } + // 如果点的距离是在增加的，且当前距离小于2倍标准，且角度小于标准，则认为是拖尾点 + else if (-1 != startI && + p.range > lastP.range && + d < maxDist * 2 && + a < maxAngle) + { + // 无处理 + } + else + { + // 判断统计位置是否有效，如果有效需要标记 + if (-1 != startI && + i - startI >= minNoise) + { + for (int j = startI; j <= i; ++j) + { + noises[j % size] = true; + const LaserPoint &pp = out.points.at(j % size); + // printf("噪点[%d] a[%.02f] r[%.02f]\n", + // j % size, ydlidar::core::math::to_degrees(pp.angle), pp.range); + } + } + + startI = -1; + } + } + lastP = p; + } + + // 处理被标记的点 + int noiseCount = 0; + for (int i = 0; i < size; ++i) + { + if (noises[i]) + { + out.points[i].range = 0.0f; + noiseCount++; + } + } + + // printf("强光过滤噪点数[%d]\n", noiseCount); +} + +StrongLightFilter::Point::Point(float x, float y) + : x(x), + y(y) +{ +} + +StrongLightFilter::Point StrongLightFilter::Point::angular2Polar( + const StrongLightFilter::Point &p) +{ + // 1.极坐标系中的两个坐标 r 和 θ 可以由下面的公式转换为直角坐标系下的坐标值x = r*cos（θ），y = r*sin（θ）。 + // 2.由上述二公式，可得到从直角坐标系中x和y两坐标如何计算出极坐标下的坐标，r = sqrt(x^2 + y^2),θ = arctan(y/x) + // float r = qSqrt(x * x + y * y); + // float theta = qAtan(y / x); + + // 将弧度值从[-M_PI/2,M_PI/2]转成[0, 2*M_PI] + float theta = .0; + if (!IS_ZERO(p.x)) // x不为0时 + { + theta = atan(p.y / p.x); + if (p.x > 0.0) + { + if (p.y < 0.0) + theta += (M_PI * 2.0); + } + else + { + theta += M_PI; + } + } + return Point(theta, sqrt(p.x * p.x + p.y * p.y)); +} + +StrongLightFilter::Point StrongLightFilter::Point::polar2Angular( + const StrongLightFilter::Point &p) +{ + return Point(p.y * cos(p.x), p.y * sin(p.x)); +} + +float StrongLightFilter::Point::calcDist( + const StrongLightFilter::Point &p, + const StrongLightFilter::Point &p1, + const StrongLightFilter::Point &p2) +{ + // 计算点到直线的最近距离 + // 输入点P(x0,y0)和直线AB（x1,y1,x2,y2）,输出点到直线的最近距离 + // # 如果两点相同，则输出一个点的坐标为垂足 + // if x1 == x2 and y1 == y2: + // return math.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)) + if (IS_EQUAL(p1.x, p2.x) && + IS_EQUAL(p1.y, p2.y)) + return sqrt((p1.x - p.x) * (p1.x - p.x) + + (p1.y - p.y) * (p1.y - p.y)); + + // # 根据向量外积计算面积 + // s = (x0 - x1) * (y2 - y1) - (y0 - y1) * (x2 - x1) + float s = (p.x - p1.x) * (p2.y - p1.y) - + (p.y - p1.y) * (p2.x - p1.x); + // # 计算直线上两点之间的距离 + // d = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + float d = sqrt((p2.x - p1.x) * (p2.x - p1.x) + + (p2.y - p1.y) * (p2.y - p1.y)); + + // return math.fabs(s / d) + return fabs(s / d); +} + +float StrongLightFilter::Point::calcLen( + const StrongLightFilter::Point &v) +{ + return sqrt(v.x * v.x + v.y * v.y); +} + +float StrongLightFilter::Point::calcDot( + const StrongLightFilter::Point &v1, + const StrongLightFilter::Point &v2) +{ + return v1.x * v2.x + v1.y * v2.y; +} + +float StrongLightFilter::Point::calcAngle( + const StrongLightFilter::Point &p1, + const StrongLightFilter::Point &p2, + const StrongLightFilter::Point &p3, + const StrongLightFilter::Point &p4) +{ + Point v1(p2.x - p1.x, p2.y - p1.y); //向量1 + Point v2(p4.x - p3.x, p4.y - p3.y); //向量2 + //计算两向量夹角（锐角） + float theta = calcDot(v1, v2) / (calcLen(v1) * calcLen(v2)); + float a = acos(theta) * 180.0f / M_PI; + if (a < .0f) + a = -a; + if (a > 90.0f) + a = 180.0f - a; + return a; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.h new file mode 100644 index 0000000..2de50c5 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/filters/StrongLightFilter.h @@ -0,0 +1,56 @@ +#ifndef STRONGLIGHTFILTER_H +#define STRONGLIGHTFILTER_H +#include "FilterInterface.h" + + +//强光滤波器（拖尾滤波器） +class YDLIDAR_API StrongLightFilter : public FilterInterface +{ +public: + StrongLightFilter(); + virtual ~StrongLightFilter(); + + virtual void filter(const LaserScan &in, + int lidarType, + int version, + LaserScan &out); + void setMaxDist(float dist) {maxDist = dist;} + void setMaxAngle(float angle) {maxAngle = angle;} + void setMinNoise(int noise) {minNoise = noise;} + +protected: + struct Point + { + float x = .0; + float y = .0; + + Point(float x = .0, float y = .0); + + static Point angular2Polar(const Point &p); // 直角坐标转极坐标 + static Point polar2Angular(const Point &p); // 极坐标转直角坐标 + // 计算直角坐标系中点到直线的距离 + static float calcDist( + const Point &p, + const Point &p1, + const Point &p2); + //计算向量的长度 + static float calcLen( + const Point &v); + //计算向量的乘积 + static float calcDot( + const Point &v1, + const Point &v2); + //计算直角坐标系中两线段组成直线的夹角 + static float calcAngle( + const Point &p1, + const Point &p2, + const Point &p3, + const Point &p4); + }; + + float maxDist = 0.05; //最大距离阈值，单位米（此值可根据需要自己修改） + float maxAngle = 12.0; //最大角度阈值，单位°（此值可根据需要自己修改） + int minNoise = 2; //最小连续噪点数（此值可根据需要自己修改） +}; + +#endif // STRONGLIGHTFILTER_H diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.cpp new file mode 100644 index 0000000..7cd212a --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.cpp @@ -0,0 +1,232 @@ +// +// 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. +// + +#include +#include "ydlidar_sdk.h" +#include "CYdLidar.h" +#include "ydlidar_config.h" + +YDLidar *lidarCreate() { + CYdLidar *lidar = new CYdLidar(); + YDLidar *instance = new YDLidar; + instance->lidar = NULL; + instance->lidar = (void *)lidar; + return instance; +} + +void lidarDestroy(YDLidar **lidar) { + if (lidar == NULL || *lidar == NULL) { + return; + } + + CYdLidar *drv = static_cast((*lidar)->lidar); + + if (drv) { + delete drv; + drv = NULL; + } + + (*lidar)->lidar = NULL; + delete *lidar; + *lidar = NULL; + return; +} + +bool setlidaropt(YDLidar *lidar, int optname, const void *optval, int optlen) { + if (lidar == NULL || lidar->lidar == NULL || optval == NULL) { + return false; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + return drv->setlidaropt(optname, optval, optlen); + } + + return false; +} + +bool getlidaropt(YDLidar *lidar, int optname, void *optval, int optlen) { + if (lidar == NULL || lidar->lidar == NULL || optval == NULL) { + return false; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + return drv->getlidaropt(optname, optval, optlen); + } + + return false; +} + + +void GetSdkVersion(char *version) { + strcpy(version, YDLIDAR_SDK_VERSION_STR); +} + +bool initialize(YDLidar *lidar) { + if (lidar == NULL || lidar->lidar == NULL) { + return false; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + return drv->initialize(); + } + + return false; +} + +void GetLidarVersion(YDLidar *lidar, LidarVersion *version) { + if (lidar == NULL || lidar->lidar == NULL || version == NULL) { + return; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + drv->GetLidarVersion(*version); + } +} + +bool turnOn(YDLidar *lidar) { + if (lidar == NULL || lidar->lidar == NULL) { + return false; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + return drv->turnOn(); + } + + return false; +} + +bool doProcessSimple(YDLidar *lidar, LaserFan *outscan) { + if (lidar == NULL || lidar->lidar == NULL || outscan == NULL) { + return false; + } + + LaserFanDestroy(outscan); + outscan->npoints = 0; + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + LaserScan scan; + bool ret = drv->doProcessSimple(scan); + outscan->config = scan.config; + outscan->stamp = scan.stamp; + outscan->npoints = scan.points.size(); + outscan->points = (LaserPoint *)malloc(sizeof(LaserPoint) * outscan->npoints); + std::copy(scan.points.begin(), scan.points.end(), outscan->points); + return ret; + } + + return false; +} + +bool turnOff(YDLidar *lidar) { + if (lidar == NULL || lidar->lidar == NULL) { + return false; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + return drv->turnOff(); + } + + return false; +} + +void disconnecting(YDLidar *lidar) { + if (lidar == NULL || lidar->lidar == NULL) { + return; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + drv->disconnecting(); + } +} + +const char *DescribeError(YDLidar *lidar) { + char const *value = ""; + + if (lidar == NULL || lidar->lidar == NULL) { + return value; + } + + CYdLidar *drv = static_cast(lidar->lidar); + + if (drv) { + return drv->DescribeError(); + } + + return value; +} + +void os_init() { + ydlidar::os_init(); +} + +bool os_isOk() { + return ydlidar::os_isOk(); +} + +void os_shutdown() { + ydlidar::os_shutdown(); + +} + +int lidarPortList(LidarPort *ports) { + if (ports == NULL) { + return 0; + } + + memset(ports, 0, sizeof(LidarPort)); + std::map lists = ydlidar::lidarPortList(); + std::map::iterator it; + + int i = 0; + + for (it = lists.begin(); it != lists.end(); it++) { + string_t port; + memset(&port, 0, sizeof(string_t)); + + if (i < sizeof(LidarPort) / sizeof(string_t)) { + memcpy(ports->port[i].data, it->second.c_str(), it->second.size()); + } + + i++; + } + + return i; + +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.h b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.h new file mode 100644 index 0000000..0043015 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/src/ydlidar_sdk.h @@ -0,0 +1,265 @@ +// +// 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. +// +/** @page YDLIDAR C API + * YDLIDAR C API + +
Library ydlidar_sdk +
File ydlidar_sdk.h +
Author Tony [code at ydlidar com] +
Source https://github.com/ydlidar/YDLidar-SDK +
Version 1.0.0 +
+ +* @copyright Copyright (c) 2018-2020 EAIBOT + Jump to the ydlidar_sdk.h interface documentation. +*/ + +#ifndef YDLIDAR_SDK_H_ +#define YDLIDAR_SDK_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +/** + * @ref "YDLIDAR_C_API" + * @par YDLIDAR_C_API + * + */ + +/** + * @brief create a Lidar instance + * @note call ::lidarDestroy destroy + * @return created instance + */ +YDLIDAR_API YDLidar *lidarCreate(void); + +/** + * @brief Destroy Lidar instance by ::lidarCreate create + * @param lidar CYdLidar instance + */ +YDLIDAR_API void lidarDestroy(YDLidar **lidar); + +/** + * @brief set lidar properties + * @param lidar a lidar instance + * @param optname option name + * @todo string properties + * - @ref LidarPropSerialPort + * - @ref LidarPropIgnoreArray + * @note set string property example + * @code + * CYdLidar laser; + * std::string lidar_port = "/dev/ydlidar"; + * laser.setlidaropt(LidarPropSerialPort,lidar_port.c_str(), lidar_port.size()); + * @endcode + * @todo int properties + * - @ref LidarPropSerialBaudrate + * - @ref LidarPropLidarType + * - @ref LidarPropDeviceType + * - @ref LidarPropSampleRate + * @note set int property example + * @code + * CYdLidar laser; + * int lidar_baudrate = 230400; + * laser.setlidaropt(LidarPropSerialPort,&lidar_baudrate, sizeof(int)); + * @endcode + * @todo bool properties + * - @ref LidarPropFixedResolution + * - @ref LidarPropReversion + * - @ref LidarPropInverted + * - @ref LidarPropAutoReconnect + * - @ref LidarPropSingleChannel + * - @ref LidarPropIntenstiy + * @note set bool property example + * @code + * CYdLidar laser; + * bool lidar_fixedresolution = true; + * laser.setlidaropt(LidarPropSerialPort,&lidar_fixedresolution, sizeof(bool)); + * @endcode + * @todo float properties + * - @ref LidarPropMaxRange + * - @ref LidarPropMinRange + * - @ref LidarPropMaxAngle + * - @ref LidarPropMinAngle + * - @ref LidarPropScanFrequency + * @note set float property example + * @code + * CYdLidar laser; + * float lidar_maxrange = 16.0f; + * laser.setlidaropt(LidarPropSerialPort,&lidar_maxrange, sizeof(float)); + * @endcode + * @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 + */ +YDLIDAR_API bool setlidaropt(YDLidar *lidar, int optname, const void *optval, + int optlen); + +/** + * @brief get lidar property + * @param lidar a lidar instance + * @param optname option name + * @todo string properties + * - @ref LidarPropSerialPort + * - @ref LidarPropIgnoreArray + * @note get string property example + * @code + * CYdLidar laser; + * char lidar_port[30]; + * laser.getlidaropt(LidarPropSerialPort,lidar_port, sizeof(lidar_port)); + * @endcode + * @todo int properties + * - @ref LidarPropSerialBaudrate + * - @ref LidarPropLidarType + * - @ref LidarPropDeviceType + * - @ref LidarPropSampleRate + * @note get int property example + * @code + * CYdLidar laser; + * int lidar_baudrate; + * laser.getlidaropt(LidarPropSerialPort,&lidar_baudrate, sizeof(int)); + * @endcode + * @todo bool properties + * - @ref LidarPropFixedResolution + * - @ref LidarPropReversion + * - @ref LidarPropInverted + * - @ref LidarPropAutoReconnect + * - @ref LidarPropSingleChannel + * - @ref LidarPropIntenstiy + * @note get bool property example + * @code + * CYdLidar laser; + * bool lidar_fixedresolution; + * laser.getlidaropt(LidarPropSerialPort,&lidar_fixedresolution, sizeof(bool)); + * @endcode + * @todo float properties + * - @ref LidarPropMaxRange + * - @ref LidarPropMinRange + * - @ref LidarPropMaxAngle + * - @ref LidarPropMinAngle + * - @ref LidarPropScanFrequency + * @note set float property example + * @code + * CYdLidar laser; + * float lidar_maxrange; + * laser.getlidaropt(LidarPropSerialPort,&lidar_maxrange, sizeof(float)); + * @endcode + * @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 + */ +YDLIDAR_API bool getlidaropt(YDLidar *lidar, int optname, void *optval, + int optlen); + +/** +* Return SDK's version information in a numeric form. +* @param version Pointer to a version for returning the version information. +*/ +YDLIDAR_API void GetSdkVersion(char *version); + +/** + * Initialize the SDK. + * @return true if successfully initialized, otherwise false. + */ +YDLIDAR_API bool initialize(YDLidar *lidar); + +/** +* @brief Return LiDAR's version information in a numeric form. +* @param version Pointer to a version structure for returning the version information. +*/ +YDLIDAR_API void GetLidarVersion(YDLidar *lidar, LidarVersion *version); + +/** + * Start the device scanning routine which runs on a separate thread. + * @return true if successfully started, otherwise false. + */ +YDLIDAR_API bool turnOn(YDLidar *lidar); + +/** + * @brief Get the LiDAR Scan Data. turnOn is successful before doProcessSimple scan data. + * @param[in] lidar LiDAR instance + * @param[out] outscan LiDAR Scan Data + * @return true if successfully started, otherwise false. + */ +YDLIDAR_API bool doProcessSimple(YDLidar *lidar, LaserFan *outscan); +/** + * @brief Stop the device scanning thread and disable motor. + * @return true if successfully Stoped, otherwise false. + */ +YDLIDAR_API bool turnOff(YDLidar *lidar); +/** + * @brief Uninitialize the SDK and Disconnect the LiDAR. + */ +YDLIDAR_API void disconnecting(YDLidar *lidar); + +/** + * @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) + */ +YDLIDAR_API const char *DescribeError(YDLidar *lidar); + +/** + * @brief initialize system signals + */ +YDLIDAR_API void os_init(); +/** + * @brief isOk + * @return true if successfully initialize, otherwise false. + */ +YDLIDAR_API bool os_isOk(); +/** + * @brief os_shutdown + */ +YDLIDAR_API void os_shutdown(); + +/** + * @brief get lidar serial port + * @param ports serial port lists + * @return valid port number + */ +YDLIDAR_API int lidarPortList(LidarPort *ports); + +#ifdef __cplusplus +} +#endif + +#endif // YDLIDAR_SDK_H_ diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/ydlidar_config.h.in b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/ydlidar_config.h.in new file mode 100644 index 0000000..c7c20c2 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/sdk/ydlidar_config.h.in @@ -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 @YDLIDAR_SDK_VERSION@ +#define YDLIDAR_SDK_VERSION_STR "@YDLIDAR_SDK_VERSION@" + diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_client.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_client.cpp new file mode 100644 index 0000000..7bac433 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_client.cpp @@ -0,0 +1,42 @@ +/* + * YDLIDAR SYSTEM + * YDLIDAR ROS 2 Node Client + * + * Copyright 2017 - 2020 EAI TEAM + * http://www.eaibot.com + * + */ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include + +#define RAD2DEG(x) ((x)*180./M_PI) + +static void scanCb(sensor_msgs::msg::LaserScan::SharedPtr scan) { + int count = scan->scan_time / scan->time_increment; + printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count); + printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min), + RAD2DEG(scan->angle_max)); + + for (int i = 0; i < count; i++) { + float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); + printf("[YDLIDAR INFO]: angle-distance : [%f, %f]\n", degree, scan->ranges[i]); + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_client"); + + auto lidar_info_sub = node->create_subscription( + "scan", rclcpp::SensorDataQoS(), scanCb); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + + return 0; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_node.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_node.cpp new file mode 100644 index 0000000..7c3701c --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_node.cpp @@ -0,0 +1,263 @@ +/* + * YDLIDAR SYSTEM + * YDLIDAR ROS 2 Node + * + * Copyright 2017 - 2020 EAI TEAM + * http://www.eaibot.com + * + */ + +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif + +#include "src/CYdLidar.h" +#include +#include +#include +#include +#include "sensor_msgs/msg/point_cloud.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "std_srvs/srv/empty.hpp" +#include +#include +#include +#include + +#define ROS2Verision "1.0.1" + + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_node"); + + RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Current ROS Driver Version: %s\n", ((std::string)ROS2Verision).c_str()); + + CYdLidar laser; + std::string str_optvalue = "/dev/ydlidar"; + node->declare_parameter("port", str_optvalue); + node->get_parameter("port", str_optvalue); + ///lidar port + laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size()); + ///ignore array + str_optvalue = ""; + node->declare_parameter("ignore_array", str_optvalue); + node->get_parameter("ignore_array", str_optvalue); + laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size()); + + std::string frame_id = "laser_frame"; + node->declare_parameter("frame_id", frame_id); + node->get_parameter("frame_id", frame_id); + + //////////////////////int property///////////////// + /// lidar baudrate + int optval = 230400; + node->declare_parameter("baudrate", optval); + node->get_parameter("baudrate", optval); + laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int)); + /// tof lidar + optval = TYPE_TRIANGLE; + node->declare_parameter("lidar_type", optval); + node->get_parameter("lidar_type", optval); + laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); + /// device type + optval = YDLIDAR_TYPE_SERIAL; + node->declare_parameter("device_type", optval); + node->get_parameter("device_type", optval); + laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); + /// sample rate + optval = 9; + node->declare_parameter("sample_rate", optval); + node->get_parameter("sample_rate", optval); + laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int)); + /// abnormal count + optval = 4; + node->declare_parameter("abnormal_check_count", optval); + node->get_parameter("abnormal_check_count", optval); + laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); + + /// Intenstiy bit count + optval = 8; + node->declare_parameter("intensity_bit", optval); + node->get_parameter("intensity_bit", optval); + laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int)); + + //////////////////////bool property///////////////// + /// fixed angle resolution + bool b_optvalue = false; + node->declare_parameter("fixed_resolution", b_optvalue); + node->get_parameter("fixed_resolution", b_optvalue); + laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); + /// rotate 180 + b_optvalue = true; + node->declare_parameter("reversion", b_optvalue); + node->get_parameter("reversion", b_optvalue); + laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool)); + /// Counterclockwise + b_optvalue = true; + node->declare_parameter("inverted", b_optvalue); + node->get_parameter("inverted", b_optvalue); + laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool)); + b_optvalue = true; + node->declare_parameter("auto_reconnect", b_optvalue); + node->get_parameter("auto_reconnect", b_optvalue); + laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool)); + /// one-way communication + b_optvalue = false; + node->declare_parameter("isSingleChannel", b_optvalue); + node->get_parameter("isSingleChannel", b_optvalue); + laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool)); + /// intensity + // b_optvalue = false; + // node->declare_parameter("intensity", b_optvalue); + // node->get_parameter("intensity", b_optvalue); + // laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); + laser.setAutoIntensity(true); + /// Motor DTR + b_optvalue = false; + node->declare_parameter("support_motor_dtr", b_optvalue); + node->get_parameter("support_motor_dtr", b_optvalue); + laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); + + //////////////////////float property///////////////// + /// unit: ° + float f_optvalue = 180.0f; + node->declare_parameter("angle_max", f_optvalue); + node->get_parameter("angle_max", f_optvalue); + laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float)); + f_optvalue = -180.0f; + node->declare_parameter("angle_min", f_optvalue); + node->get_parameter("angle_min", f_optvalue); + laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float)); + /// unit: m + f_optvalue = 64.f; + node->declare_parameter("range_max", f_optvalue); + node->get_parameter("range_max", f_optvalue); + laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float)); + f_optvalue = 0.1f; + node->declare_parameter("range_min", f_optvalue); + node->get_parameter("range_min", f_optvalue); + laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float)); + /// unit: Hz + f_optvalue = 10.f; + node->declare_parameter("frequency", f_optvalue); + node->get_parameter("frequency", f_optvalue); + laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float)); + + bool invalid_range_is_inf = false; + node->declare_parameter("invalid_range_is_inf", invalid_range_is_inf); + node->get_parameter("invalid_range_is_inf", invalid_range_is_inf); + + + bool ret = laser.initialize(); + if (ret) { + ret = laser.turnOn(); + } else { + RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError()); + } + + auto laser_pub = node->create_publisher("scan", rclcpp::SensorDataQoS()); + auto pc_pub = node->create_publisher("point_cloud", rclcpp::SensorDataQoS()); + + auto stop_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOff(); + }; + + auto stop_service = node->create_service("stop_scan",stop_scan_service); + + auto start_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOn(); + }; + + auto start_service = node->create_service("start_scan",start_scan_service); + + rclcpp::WallRate loop_rate(20); + + while (ret && rclcpp::ok()) { + + LaserScan scan;// + + if (laser.doProcessSimple(scan)) { + + auto scan_msg = std::make_shared(); + auto pc_msg = std::make_shared(); + + scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp); + scan_msg->header.stamp.nanosec = scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec); + scan_msg->header.frame_id = frame_id; + pc_msg->header = scan_msg->header; + 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); + + pc_msg->channels.resize(2); + int idx_intensity = 0; + pc_msg->channels[idx_intensity].name = "intensities"; + int idx_timestamp = 1; + pc_msg->channels[idx_timestamp].name = "stamps"; + + for(size_t 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) { + if (scan.points[i].range >= scan.config.min_range) { + scan_msg->ranges[index] = scan.points[i].range; + scan_msg->intensities[index] = scan.points[i].intensity; + } + } + + if (scan.points[i].range >= scan.config.min_range && + scan.points[i].range <= scan.config.max_range) { + geometry_msgs::msg::Point32 point; + point.x = scan.points[i].range * cos(scan.points[i].angle); + point.y = scan.points[i].range * sin(scan.points[i].angle); + point.z = 0.0; + pc_msg->points.push_back(point); + pc_msg->channels[idx_intensity].values.push_back(scan.points[i].intensity); + pc_msg->channels[idx_timestamp].values.push_back(i * scan.config.time_increment); + } + + } + + laser_pub->publish(*scan_msg); + pc_pub->publish(*pc_msg); + + } else { + RCLCPP_ERROR(node->get_logger(), "Failed to get scan"); + } + if(!rclcpp::ok()) { + break; + } + rclcpp::spin_some(node); + loop_rate.sleep(); + } + + + RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping ......."); + laser.turnOff(); + laser.disconnecting(); + rclcpp::shutdown(); + + return 0; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_client.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_client.cpp new file mode 100755 index 0000000..7bac433 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_client.cpp @@ -0,0 +1,42 @@ +/* + * YDLIDAR SYSTEM + * YDLIDAR ROS 2 Node Client + * + * Copyright 2017 - 2020 EAI TEAM + * http://www.eaibot.com + * + */ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include + +#define RAD2DEG(x) ((x)*180./M_PI) + +static void scanCb(sensor_msgs::msg::LaserScan::SharedPtr scan) { + int count = scan->scan_time / scan->time_increment; + printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count); + printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min), + RAD2DEG(scan->angle_max)); + + for (int i = 0; i < count; i++) { + float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); + printf("[YDLIDAR INFO]: angle-distance : [%f, %f]\n", degree, scan->ranges[i]); + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_client"); + + auto lidar_info_sub = node->create_subscription( + "scan", rclcpp::SensorDataQoS(), scanCb); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + + return 0; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_node.cpp b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_node.cpp new file mode 100755 index 0000000..7c3701c --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/src/ydlidar_ros2_driver_node.cpp @@ -0,0 +1,263 @@ +/* + * YDLIDAR SYSTEM + * YDLIDAR ROS 2 Node + * + * Copyright 2017 - 2020 EAI TEAM + * http://www.eaibot.com + * + */ + +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif + +#include "src/CYdLidar.h" +#include +#include +#include +#include +#include "sensor_msgs/msg/point_cloud.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "std_srvs/srv/empty.hpp" +#include +#include +#include +#include + +#define ROS2Verision "1.0.1" + + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_node"); + + RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Current ROS Driver Version: %s\n", ((std::string)ROS2Verision).c_str()); + + CYdLidar laser; + std::string str_optvalue = "/dev/ydlidar"; + node->declare_parameter("port", str_optvalue); + node->get_parameter("port", str_optvalue); + ///lidar port + laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size()); + ///ignore array + str_optvalue = ""; + node->declare_parameter("ignore_array", str_optvalue); + node->get_parameter("ignore_array", str_optvalue); + laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size()); + + std::string frame_id = "laser_frame"; + node->declare_parameter("frame_id", frame_id); + node->get_parameter("frame_id", frame_id); + + //////////////////////int property///////////////// + /// lidar baudrate + int optval = 230400; + node->declare_parameter("baudrate", optval); + node->get_parameter("baudrate", optval); + laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int)); + /// tof lidar + optval = TYPE_TRIANGLE; + node->declare_parameter("lidar_type", optval); + node->get_parameter("lidar_type", optval); + laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); + /// device type + optval = YDLIDAR_TYPE_SERIAL; + node->declare_parameter("device_type", optval); + node->get_parameter("device_type", optval); + laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); + /// sample rate + optval = 9; + node->declare_parameter("sample_rate", optval); + node->get_parameter("sample_rate", optval); + laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int)); + /// abnormal count + optval = 4; + node->declare_parameter("abnormal_check_count", optval); + node->get_parameter("abnormal_check_count", optval); + laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); + + /// Intenstiy bit count + optval = 8; + node->declare_parameter("intensity_bit", optval); + node->get_parameter("intensity_bit", optval); + laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int)); + + //////////////////////bool property///////////////// + /// fixed angle resolution + bool b_optvalue = false; + node->declare_parameter("fixed_resolution", b_optvalue); + node->get_parameter("fixed_resolution", b_optvalue); + laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); + /// rotate 180 + b_optvalue = true; + node->declare_parameter("reversion", b_optvalue); + node->get_parameter("reversion", b_optvalue); + laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool)); + /// Counterclockwise + b_optvalue = true; + node->declare_parameter("inverted", b_optvalue); + node->get_parameter("inverted", b_optvalue); + laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool)); + b_optvalue = true; + node->declare_parameter("auto_reconnect", b_optvalue); + node->get_parameter("auto_reconnect", b_optvalue); + laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool)); + /// one-way communication + b_optvalue = false; + node->declare_parameter("isSingleChannel", b_optvalue); + node->get_parameter("isSingleChannel", b_optvalue); + laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool)); + /// intensity + // b_optvalue = false; + // node->declare_parameter("intensity", b_optvalue); + // node->get_parameter("intensity", b_optvalue); + // laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); + laser.setAutoIntensity(true); + /// Motor DTR + b_optvalue = false; + node->declare_parameter("support_motor_dtr", b_optvalue); + node->get_parameter("support_motor_dtr", b_optvalue); + laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); + + //////////////////////float property///////////////// + /// unit: ° + float f_optvalue = 180.0f; + node->declare_parameter("angle_max", f_optvalue); + node->get_parameter("angle_max", f_optvalue); + laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float)); + f_optvalue = -180.0f; + node->declare_parameter("angle_min", f_optvalue); + node->get_parameter("angle_min", f_optvalue); + laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float)); + /// unit: m + f_optvalue = 64.f; + node->declare_parameter("range_max", f_optvalue); + node->get_parameter("range_max", f_optvalue); + laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float)); + f_optvalue = 0.1f; + node->declare_parameter("range_min", f_optvalue); + node->get_parameter("range_min", f_optvalue); + laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float)); + /// unit: Hz + f_optvalue = 10.f; + node->declare_parameter("frequency", f_optvalue); + node->get_parameter("frequency", f_optvalue); + laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float)); + + bool invalid_range_is_inf = false; + node->declare_parameter("invalid_range_is_inf", invalid_range_is_inf); + node->get_parameter("invalid_range_is_inf", invalid_range_is_inf); + + + bool ret = laser.initialize(); + if (ret) { + ret = laser.turnOn(); + } else { + RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError()); + } + + auto laser_pub = node->create_publisher("scan", rclcpp::SensorDataQoS()); + auto pc_pub = node->create_publisher("point_cloud", rclcpp::SensorDataQoS()); + + auto stop_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOff(); + }; + + auto stop_service = node->create_service("stop_scan",stop_scan_service); + + auto start_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOn(); + }; + + auto start_service = node->create_service("start_scan",start_scan_service); + + rclcpp::WallRate loop_rate(20); + + while (ret && rclcpp::ok()) { + + LaserScan scan;// + + if (laser.doProcessSimple(scan)) { + + auto scan_msg = std::make_shared(); + auto pc_msg = std::make_shared(); + + scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp); + scan_msg->header.stamp.nanosec = scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec); + scan_msg->header.frame_id = frame_id; + pc_msg->header = scan_msg->header; + 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); + + pc_msg->channels.resize(2); + int idx_intensity = 0; + pc_msg->channels[idx_intensity].name = "intensities"; + int idx_timestamp = 1; + pc_msg->channels[idx_timestamp].name = "stamps"; + + for(size_t 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) { + if (scan.points[i].range >= scan.config.min_range) { + scan_msg->ranges[index] = scan.points[i].range; + scan_msg->intensities[index] = scan.points[i].intensity; + } + } + + if (scan.points[i].range >= scan.config.min_range && + scan.points[i].range <= scan.config.max_range) { + geometry_msgs::msg::Point32 point; + point.x = scan.points[i].range * cos(scan.points[i].angle); + point.y = scan.points[i].range * sin(scan.points[i].angle); + point.z = 0.0; + pc_msg->points.push_back(point); + pc_msg->channels[idx_intensity].values.push_back(scan.points[i].intensity); + pc_msg->channels[idx_timestamp].values.push_back(i * scan.config.time_increment); + } + + } + + laser_pub->publish(*scan_msg); + pc_pub->publish(*pc_msg); + + } else { + RCLCPP_ERROR(node->get_logger(), "Failed to get scan"); + } + if(!rclcpp::ok()) { + break; + } + rclcpp::spin_some(node); + loop_rate.sleep(); + } + + + RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping ......."); + laser.turnOff(); + laser.disconnecting(); + rclcpp::shutdown(); + + return 0; +} diff --git a/chapt9/fishbot_ws/src/ydlidar_ros2/startup/initenv.sh b/chapt9/fishbot_ws/src/ydlidar_ros2/startup/initenv.sh new file mode 100755 index 0000000..9b5c1d0 --- /dev/null +++ b/chapt9/fishbot_ws/src/ydlidar_ros2/startup/initenv.sh @@ -0,0 +1,11 @@ +#!/bin/bash +echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules + +echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-V2.rules + +echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-2303.rules + +service udev reload +sleep 2 +service udev restart +