diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 37463e4..351fb00 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -23,7 +23,9 @@ jobs: with: persist-credentials: false - name: Install coverage dependencies - run: sudo apt-get update && sudo apt-get install -y gcovr libgtest-dev + run: >- + sudo apt-get update && sudo apt-get install -y + gcovr libcurl4-openssl-dev libgtest-dev - name: Configure coverage build run: >- cmake -S . -B build/coverage diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d862dc4..2b356ef 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package netft_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.0 (2026-07-22) +------------------ +* Add a private ``netft-cpp`` v0.1.2 core snapshot and remove the duplicate original runtime. +* Read sensor-reported calibration by default and convert its native force and torque units to N and Nm. +* Add the explicit ``use_sensor_calibration=false`` manual override, with counts/N and counts/Nm inputs. +* Add HTTP configuration port and timeout parameters for sensor calibration discovery. +* Support Noetic's GCC 9 standard library with an Apache-2.0 XML parser compatibility translation unit. +* Keep the ros2_control plugin ABI private by hiding core and implementation symbols. +* License the complete repository under Apache-2.0, matching the upstream core. + 0.2.2 (2026-07-22) ------------------ * Replace the project-specific sensor address in defaults and examples with diff --git a/CMakeLists.txt b/CMakeLists.txt index 37a120a..e22cd5d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.16) project(netft_driver) set(CMAKE_CXX_STANDARD 17) @@ -8,20 +8,74 @@ set(CMAKE_CXX_EXTENSIONS OFF) option(NETFT_CORE_ONLY "Build only the ROS-neutral C++ core" OFF) option(NETFT_INSTALL_TEST_TOOLS "Install private integration-test tools" OFF) -add_library(netft_core STATIC src/protocol.cpp src/status.cpp src/client.cpp) +find_package(Threads REQUIRED) +find_package(CURL 7.63 REQUIRED) + +include(CheckCXXSourceCompiles) +check_cxx_source_compiles( + [=[ + #include + #include + + int main() { + const char input[] = "1"; + double value{}; + const auto result = + std::from_chars(input, input + 1, value, std::chars_format::general); + return result.ec == std::errc{} ? 0 : 1; + } + ]=] + NETFT_HAS_FLOAT_FROM_CHARS) + +set(NETFT_SNAPSHOT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/core") +if(NETFT_HAS_FLOAT_FROM_CHARS) + set(NETFT_XML_CONFIG_SOURCE + "${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/xml_config.cpp") +else() + set(NETFT_XML_CONFIG_SOURCE + "${CMAKE_CURRENT_SOURCE_DIR}/src/compat/xml_config.cpp") +endif() +set(NETFT_SNAPSHOT_SOURCES + src/core/src/types.cpp + src/core/src/status.cpp + src/core/src/discovery.cpp + src/core/src/client.cpp + src/core/src/detail/client_impl.cpp + src/core/src/detail/fault_latch.cpp + src/core/src/detail/posix_transport.cpp + src/core/src/detail/protocol.cpp + src/core/src/detail/sequence.cpp + ${NETFT_XML_CONFIG_SOURCE} +) + +add_library(netft_core STATIC ${NETFT_SNAPSHOT_SOURCES}) set_target_properties(netft_core PROPERTIES POSITION_INDEPENDENT_CODE ON) target_compile_features(netft_core PUBLIC cxx_std_17) -target_include_directories(netft_core PUBLIC - $ +target_compile_definitions(netft_core PRIVATE NETFT_BUILDING_LIBRARY) +target_include_directories(netft_core + PUBLIC ${NETFT_SNAPSHOT_SOURCE_DIR}/include + PRIVATE ${NETFT_SNAPSHOT_SOURCE_DIR}/src ) +target_link_libraries(netft_core PUBLIC Threads::Threads CURL::libcurl) + +add_library(netft_ros_support STATIC + src/ros/unit_conversion.cpp + src/ros/diagnostics.cpp) +set_target_properties(netft_ros_support PROPERTIES POSITION_INDEPENDENT_CODE ON) +target_compile_features(netft_ros_support PUBLIC cxx_std_17) +target_include_directories(netft_ros_support + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${NETFT_SNAPSHOT_SOURCE_DIR}/include) +target_link_libraries(netft_ros_support PUBLIC netft_core) add_executable(netft_check src/check_main.cpp) -find_package(Threads REQUIRED) -target_link_libraries(netft_check PRIVATE netft_core Threads::Threads) +target_link_libraries(netft_check PRIVATE netft_ros_support netft_core) if(NETFT_CORE_ONLY) enable_testing() find_package(GTest REQUIRED) + add_subdirectory(test/core) add_subdirectory(test/cpp) return() endif() @@ -46,12 +100,13 @@ endif() if(NETFT_ROS_VERSION STREQUAL "1") find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs std_srvs diagnostic_msgs) catkin_package( - INCLUDE_DIRS include - LIBRARIES netft_core CATKIN_DEPENDS roscpp geometry_msgs std_srvs diagnostic_msgs) add_executable(netft_node_cpp src/ros1_node.cpp) set_target_properties(netft_node_cpp PROPERTIES OUTPUT_NAME netft_node) - target_link_libraries(netft_node_cpp PRIVATE netft_core ${catkin_LIBRARIES}) + target_link_libraries(netft_node_cpp PRIVATE + netft_ros_support + netft_core + ${catkin_LIBRARIES}) target_include_directories(netft_node_cpp PRIVATE ${catkin_INCLUDE_DIRS}) if(CATKIN_ENABLE_TESTING) catkin_run_tests_target( @@ -73,15 +128,15 @@ if(NETFT_ROS_VERSION STREQUAL "1") FILES config/netft_ros1.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config ) - install(TARGETS netft_core ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(TARGETS netft_node_cpp netft_check RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(netft_ros1_node test/cpp/test_ros1_node.cpp) if(TARGET netft_ros1_node) - target_link_libraries(netft_ros1_node netft_core ${catkin_LIBRARIES}) + target_link_libraries(netft_ros1_node + netft_ros_support + netft_core + ${catkin_LIBRARIES}) target_include_directories(netft_ros1_node PRIVATE ${catkin_INCLUDE_DIRS}) endif() endif() @@ -99,25 +154,42 @@ elseif(NETFT_ROS_VERSION STREQUAL "2") else() set(NETFT_ROS2_CONTROL_API_DEFINITION NETFT_ROS2_CONTROL_MODERN_API) endif() + + function(netft_configure_ros2_control_target target) + set_target_properties(${target} PROPERTIES + CXX_VISIBILITY_PRESET hidden + VISIBILITY_INLINES_HIDDEN YES) + target_compile_definitions(${target} PRIVATE + ${NETFT_ROS2_CONTROL_API_DEFINITION}) + target_include_directories(${target} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src) + target_link_libraries(${target} PRIVATE + netft_ros_support + netft_core + ${hardware_interface_TARGETS} + ${pluginlib_TARGETS} + ${rclcpp_TARGETS} + ${realtime_tools_TARGETS} + ${std_srvs_TARGETS} + ${diagnostic_msgs_TARGETS} + ) + if( + CMAKE_SYSTEM_NAME STREQUAL "Linux" + AND CMAKE_CXX_COMPILER_ID MATCHES "^(GNU|Clang)$" + ) + target_link_options(${target} PRIVATE + "LINKER:--exclude-libs,libnetft_core.a:libnetft_ros_support.a") + endif() + endfunction() + add_library(netft_ros2_control SHARED src/netft_hardware_interface.cpp) - target_compile_definitions(netft_ros2_control PRIVATE - ${NETFT_ROS2_CONTROL_API_DEFINITION}) - target_include_directories(netft_ros2_control PUBLIC - $) - target_link_libraries(netft_ros2_control PRIVATE - netft_core - ${hardware_interface_TARGETS} - ${pluginlib_TARGETS} - ${rclcpp_TARGETS} - ${realtime_tools_TARGETS} - ${std_srvs_TARGETS} - ${diagnostic_msgs_TARGETS} - ) + netft_configure_ros2_control_target(netft_ros2_control) pluginlib_export_plugin_description_file( hardware_interface netft_hardware_plugins.xml) add_executable(netft_node_cpp src/ros2_node.cpp) set_target_properties(netft_node_cpp PROPERTIES OUTPUT_NAME netft_node) target_link_libraries(netft_node_cpp PRIVATE + netft_ros_support netft_core ${rclcpp_TARGETS} ${geometry_msgs_TARGETS} @@ -163,13 +235,49 @@ elseif(NETFT_ROS_VERSION STREQUAL "2") ) endif() if(BUILD_TESTING) - target_compile_definitions(netft_ros2_control PRIVATE - NETFT_ROS2_CONTROL_TESTING) find_package(ament_cmake_pytest REQUIRED) find_package(ament_cmake_gtest REQUIRED) + + set( + NETFT_ROS2_CONTROL_TEST_PREFIX + "${CMAKE_CURRENT_BINARY_DIR}/netft_ros2_control_test_prefix") + set( + NETFT_ROS2_CONTROL_TEST_SHARE + "${NETFT_ROS2_CONTROL_TEST_PREFIX}/share/${PROJECT_NAME}") + file(MAKE_DIRECTORY + "${NETFT_ROS2_CONTROL_TEST_PREFIX}/lib" + "${NETFT_ROS2_CONTROL_TEST_PREFIX}/share/ament_index/resource_index/packages" + "${NETFT_ROS2_CONTROL_TEST_PREFIX}/share/ament_index/resource_index/hardware_interface__pluginlib__plugin" + "${NETFT_ROS2_CONTROL_TEST_SHARE}") + configure_file( + test/cpp/netft_hardware_test_plugins.xml + "${NETFT_ROS2_CONTROL_TEST_SHARE}/netft_hardware_test_plugins.xml" + COPYONLY) + configure_file( + package.xml + "${NETFT_ROS2_CONTROL_TEST_SHARE}/package.xml" + COPYONLY) + file(WRITE + "${NETFT_ROS2_CONTROL_TEST_PREFIX}/share/ament_index/resource_index/packages/${PROJECT_NAME}" + "") + file(WRITE + "${NETFT_ROS2_CONTROL_TEST_PREFIX}/share/ament_index/resource_index/hardware_interface__pluginlib__plugin/${PROJECT_NAME}" + "share/${PROJECT_NAME}/netft_hardware_test_plugins.xml") + + add_library( + netft_ros2_control_testing SHARED + src/netft_hardware_interface.cpp) + netft_configure_ros2_control_target(netft_ros2_control_testing) + target_compile_definitions(netft_ros2_control_testing PRIVATE + NETFT_ROS2_CONTROL_TESTING) + set_target_properties(netft_ros2_control_testing PROPERTIES + LIBRARY_OUTPUT_DIRECTORY "${NETFT_ROS2_CONTROL_TEST_PREFIX}/lib" + RUNTIME_OUTPUT_DIRECTORY "${NETFT_ROS2_CONTROL_TEST_PREFIX}/lib") + ament_add_gtest(netft_ros2_node test/cpp/test_ros2_node.cpp) if(TARGET netft_ros2_node) target_link_libraries(netft_ros2_node + netft_ros_support netft_core ${rclcpp_TARGETS} ${geometry_msgs_TARGETS} @@ -179,26 +287,43 @@ elseif(NETFT_ROS_VERSION STREQUAL "2") endif() ament_add_gtest(netft_hardware_interface test/cpp/test_hardware_interface.cpp - test/cpp/support/fake_sensor.cpp + test/cpp/support/ros2_control_test_access.cpp + test/core/support/fake_http_server.cpp + test/core/support/fake_sensor.cpp + ENV + "AMENT_PREFIX_PATH=${NETFT_ROS2_CONTROL_TEST_PREFIX}:$ENV{AMENT_PREFIX_PATH}" ) if(TARGET netft_hardware_interface) target_compile_definitions(netft_hardware_interface PRIVATE - ${NETFT_ROS2_CONTROL_API_DEFINITION} - NETFT_ROS2_CONTROL_TESTING) + ${NETFT_ROS2_CONTROL_API_DEFINITION}) + target_include_directories(netft_hardware_interface PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/test/core + ${NETFT_SNAPSHOT_SOURCE_DIR}/src) target_link_libraries(netft_hardware_interface - netft_ros2_control + netft_ros_support netft_core + ${CMAKE_DL_LIBS} ${hardware_interface_TARGETS} ${rclcpp_TARGETS} ${std_srvs_TARGETS} ${diagnostic_msgs_TARGETS} ) + add_dependencies( + netft_hardware_interface + netft_ros2_control_testing) endif() ament_add_pytest_test(netft_unit test WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} TIMEOUT 90 ) + add_test( + NAME netft_ros2_control_symbol_audit + COMMAND + "${CMAKE_CURRENT_SOURCE_DIR}/test/integration/audit_ros2_control_symbols.sh" + "$" + ) ament_add_pytest_test(netft_ros2_smoke_harness test/test_shell_harness.py WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} @@ -218,9 +343,7 @@ elseif(NETFT_ROS_VERSION STREQUAL "2") TIMEOUT 30 ) endif() - install(TARGETS netft_core ARCHIVE DESTINATION lib) install(TARGETS netft_node_cpp netft_check DESTINATION lib/${PROJECT_NAME}) - install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) install(TARGETS netft_ros2_control ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index f1455a9..1ffc4ab 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -66,6 +66,18 @@ Before opening a pull request: Open an issue before starting a large feature or an interface-breaking change so the scope can be agreed upon first. +## Core snapshot updates + +1. Update and release `netft-cpp` first. +2. Copy only the library paths from an immutable upstream tag. +3. Preserve the `netft` namespace and the Apache-2.0 license. +4. Update `src/core/UPSTREAM` with the copied tag and commit. +5. Keep the Apache-2.0 `src/compat/xml_config.cpp` fallback synchronized with the copied upstream XML parser. +6. Rerun the byte comparison and every snapshot and ROS test. +7. Keep ROS-specific changes out of `src/core/`; port reusable fixes upstream before copying them back. + +Do not add an automated downloader or submodule for the core snapshot. + ## Physical sensor safety The automated test suite does not require a physical sensor. Do not perform @@ -80,5 +92,8 @@ issued. Do not publish private network details or sensor identifiers. ## License -By contributing, you agree that your contribution is licensed under the -repository's [MIT License](LICENSE). +Contributions are licensed under the repository's +[Apache-2.0 License](LICENSE). The copied `src/core/` snapshot retains the +upstream license copy at [src/core/LICENSE](src/core/LICENSE). Propose reusable +core fixes upstream first, then copy them from a released immutable upstream +revision and update the fallback when the XML parser changes. diff --git a/LICENSE b/LICENSE index c832686..261eeb9 100644 --- a/LICENSE +++ b/LICENSE @@ -1,21 +1,201 @@ -MIT License - -Copyright (c) 2026 ros-netft 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. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md index 21f173b..84a53a7 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ [![CI](https://github.com/netft/ros-netft/actions/workflows/ci.yml/badge.svg?branch=main)](https://github.com/netft/ros-netft/actions/workflows/ci.yml) [![Codecov](https://codecov.io/gh/netft/ros-netft/graph/badge.svg?branch=main)](https://app.codecov.io/gh/netft/ros-netft) [![ROS](https://img.shields.io/badge/ROS-1%20%7C%202-22314E.svg?logo=ros&logoColor=white)](https://www.ros.org/) -[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](LICENSE) +[![License: Apache-2.0](https://img.shields.io/badge/License-Apache--2.0-blue.svg)](LICENSE) `netft_driver` acquires force and torque data from ATI Ethernet Net F/T and Ethernet Axia sensors over the UDP Raw Data Transfer (RDT) protocol. A native @@ -131,6 +131,10 @@ wrist_ft_broadcaster: frame_id: wrist_ft_link ``` +That YAML configures only the controller manager and broadcaster. Hardware +endpoint, calibration, and timeout values come from the robot description's +Xacro-generated `` element. + Spawn the broadcaster through the controller manager used by the robot: ```bash @@ -226,10 +230,14 @@ unless `publish_on_error=true`. | `frame_id` | `netft_link` | Wrench header frame | | `wrench_topic` | `/netft/wrench` | Output topic | | `bias_service` | `/netft/bias` | Software-bias service | -| `counts_per_force` | `1000000.0` | Counts per N | -| `counts_per_torque` | `1000000.0` | Counts per Nm | +| `http_port` | `80` | Sensor HTTP configuration port | +| `use_sensor_calibration` | `true` | Read calibration and native units from the sensor over HTTP | +| `counts_per_force` | `1000000.0` | Counts per N when `use_sensor_calibration=false` | +| `counts_per_torque` | `1000000.0` | Counts per Nm when `use_sensor_calibration=false` | | `publish_rate` | `0.0` | Zero publishes every sample; a positive value limits Hz | | `receive_timeout` | `0.1` | Seconds without a valid record before recovery | +| `configuration_connect_timeout` | `0.5` | HTTP connection timeout in seconds | +| `configuration_timeout` | `1.0` | Total HTTP configuration-request timeout in seconds | | `reconnect_initial_delay` | `0.25` | Initial recovery delay in seconds | | `reconnect_max_delay` | `5.0` | Maximum recovery delay in seconds | | `diagnostics_rate` | `1.0` | Diagnostics Hz | @@ -239,14 +247,20 @@ unless `publish_on_error=true`. ### ros2_control parameters -The Xacro exposes the endpoint, independent force and torque scales, -`receive_timeout`, and `activation_timeout`. Additional hardware parameters -may be placed in the generated `` element: +The Xacro exposes the RDT and HTTP endpoints, calibration controls, +`receive_timeout`, configuration timeouts, and `activation_timeout`. Additional +hardware parameters may be placed in the generated `` element: | Parameter | Default | Meaning | |---|---:|---| | `activation_timeout` | `2.0` | Seconds to wait for the first healthy sample | | `bias_service` | `//bias` | Instance-specific software-bias service; see the encoding rule above | +| `http_port` | `80` | Sensor HTTP configuration port | +| `use_sensor_calibration` | `true` | Read calibration and native units from the sensor over HTTP | +| `counts_per_force` | `1000000.0` | Counts per N when `use_sensor_calibration=false` | +| `counts_per_torque` | `1000000.0` | Counts per Nm when `use_sensor_calibration=false` | +| `configuration_connect_timeout` | `0.5` | HTTP connection timeout in seconds | +| `configuration_timeout` | `1.0` | Total HTTP configuration-request timeout in seconds | | `diagnostics_rate` | `1.0` | Diagnostics publication rate in Hz | | `expected_rdt_rate` | `2000.0` | Expected sensor receive rate in Hz | | `rate_tolerance` | `0.2` | Allowed fractional receive-rate deviation | @@ -255,6 +269,14 @@ may be placed in the generated `` element: `ros2_control`. Standalone reconnect-delay and `publish_on_error` parameters do not apply to the fail-stop plugin. +### Calibration and HTTP configuration + +Automatic sensor configuration is enabled by default. Before streaming, the driver requests the sensor configuration from `http://:/netftapi2.xml`, using `http_port`, `configuration_connect_timeout`, and `configuration_timeout`. It uses the sensor-reported counts and native force and torque units, then converts every published wrench to N and Nm. + +Set `use_sensor_calibration=false` only when an explicit manual calibration is required. This disables HTTP discovery completely and makes `counts_per_force` and `counts_per_torque` the complete calibration override: they are counts/N and counts/Nm, respectively. The override is always interpreted as N and Nm; do not enter counts for another native unit. + +With automatic configuration, an unreachable HTTP endpoint, connection or total timeout, non-200 response, oversized response, or invalid configuration prevents a session from starting. The standalone driver reports the configuration failure in diagnostics and retries through its normal reconnect backoff. The `ros2_control` plugin is fail-stop, so activation fails until the configuration issue is corrected and the component is activated again. + ## Operations and safety On normal shutdown the driver sends Stop Streaming (`0x0000`) and closes its @@ -320,6 +342,10 @@ pixi run -e humble ros2-control-smoke See [Architecture](docs/architecture.md) for implementation and lifecycle boundaries. +## Core provenance and licenses + +`src/core/` is a private, immutable snapshot of the `netft-cpp` v0.1.2 library. It is included in this source package and does not require an external `netft-cpp` build dependency. The snapshot retains the upstream `netft` namespace. On standard libraries without floating-point `std::from_chars`, the build selects the `src/compat/xml_config.cpp` compatibility translation unit. + ## Contributing Read [CONTRIBUTING.md](CONTRIBUTING.md) before opening an @@ -328,4 +354,4 @@ Read [CONTRIBUTING.md](CONTRIBUTING.md) before opening an ## License -MIT. See [LICENSE](LICENSE). +The entire repository is licensed under Apache-2.0; see [LICENSE](LICENSE). The private core snapshot retains its upstream license copy at [src/core/LICENSE](src/core/LICENSE). diff --git a/config/netft_ros1.yaml b/config/netft_ros1.yaml index ec08da0..97f0aa1 100644 --- a/config/netft_ros1.yaml +++ b/config/netft_ros1.yaml @@ -1,5 +1,7 @@ sensor_ip: 192.168.1.1 sensor_port: 49152 +http_port: 80 +use_sensor_calibration: true frame_id: netft_link wrench_topic: /netft/wrench bias_service: /netft/bias @@ -7,6 +9,8 @@ counts_per_force: 1000000.0 counts_per_torque: 1000000.0 publish_rate: 0.0 receive_timeout: 0.1 +configuration_connect_timeout: 0.5 +configuration_timeout: 1.0 reconnect_initial_delay: 0.25 reconnect_max_delay: 5.0 diagnostics_rate: 1.0 diff --git a/config/netft_ros2.yaml b/config/netft_ros2.yaml index b8baa62..3c8fa75 100644 --- a/config/netft_ros2.yaml +++ b/config/netft_ros2.yaml @@ -2,6 +2,8 @@ netft: ros__parameters: sensor_ip: 192.168.1.1 sensor_port: 49152 + http_port: 80 + use_sensor_calibration: true frame_id: netft_link wrench_topic: /netft/wrench bias_service: /netft/bias @@ -9,6 +11,8 @@ netft: counts_per_torque: 1000000.0 publish_rate: 0.0 receive_timeout: 0.1 + configuration_connect_timeout: 0.5 + configuration_timeout: 1.0 reconnect_initial_delay: 0.25 reconnect_max_delay: 5.0 diagnostics_rate: 1.0 diff --git a/docs/architecture.md b/docs/architecture.md index 24fee87..63a23d1 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -6,31 +6,34 @@ operator procedures, and safety guidance. ## Package boundaries -The package contains one ROS-neutral C++17 core and three adapters: +The active transport implementation is a private snapshot of netft-cpp under `src/core`. It is built as `netft_core`, uses the `netft` namespace, contains no ROS headers, and is not installed as a public SDK. When the C++ standard library lacks floating-point `std::from_chars`, CMake substitutes the Apache-2.0 `src/compat/xml_config.cpp` translation unit for the snapshot XML parser. `netft::Client` is the single client used by the command-line tool, standalone nodes, and ros2_control plugin. ```text -netft_core -├── src/protocol.cpp RDT wire encoding and decoding -├── src/status.cpp status, sequence, and diagnostic policy -└── src/client.cpp UDP transport, acquisition, and recovery - -src/check_main.cpp bounded command-line endpoint check -src/ros1_node.cpp roscpp standalone adapter -src/ros2_node.cpp rclcpp standalone adapter +src/core sources +└── netft_core + └── netft_ros_support + ├── netft_check + ├── netft_node (ROS 1 or ROS 2) + └── netft_ros2_control (ROS 2 only) + +src/ros/unit_conversion.cpp native engineering units to N and Nm +src/ros/diagnostics.cpp ROS-facing health evaluation src/netft_hardware_interface.cpp - ros2_control SensorInterface plugin + ros2_control SensorInterface implementation ``` -`netft_core` contains no ROS headers and owns the single transport and fault -implementation used by every adapter. The build selects either the ROS 1 or -ROS 2 standalone adapter and installs it as `netft_node`. `netft_check` is a -native executable that performs a bounded acquisition without ROS graph -traffic or a software-bias command. +`netft_ros_support` is the boundary between the private snapshot and the adapters. It owns SI conversion and ROS-facing diagnostic interpretation but no transport. The build selects either the ROS 1 or ROS 2 standalone adapter and installs it as `netft_node`. `netft_check` performs a bounded acquisition without ROS graph traffic or a software-bias command. -The ROS 2 build additionally exports -`netft_driver/NetFTHardwareInterface` through `netft_hardware_plugins.xml`. -The plugin is one consumer of a robot's existing controller manager; it does -not implement a controller or require a second controller manager. +`netft_core`, `netft_ros_support`, and their headers remain private and are not +installed. The ROS 2 build installs the uninstrumented +`netft_ros2_control` plugin as `netft_driver/NetFTHardwareInterface` through +`netft_hardware_plugins.xml`; core and support archive symbols are localized +at that DSO boundary. When tests are enabled, a separate non-installed +`netft_ros2_control_testing` plugin is compiled with private hooks and loaded +through a build-tree-only plugin index; the production plugin is never +compiled with those hooks. + +The plugin is one consumer of a robot's existing controller manager; it does not implement a controller or require a second controller manager. ## Protocol and transport core @@ -39,12 +42,9 @@ Records contain the RDT sequence, FT sequence, device status, three signed force counts, and three signed torque counts. All fields use network byte order, and force and torque use independent counts-per-unit scales. -`src/protocol.cpp` owns exact request construction and 36-byte record parsing. -`src/status.cpp` owns device-status classification, unsigned 32-bit RDT -sequence comparison, FT progress and restart confirmation, diagnostic report -construction, and bounded fault-log throttling. `src/client.cpp` owns endpoint -resolution, the UDP socket, streaming commands, the receiver thread, unit -conversion, health counters, and the latest complete sample. +Within the private snapshot, `src/core/src/detail/protocol.cpp` owns exact request construction and 36-byte record parsing, the sequence and fault-latch components own RDT/FT progress and first-fault semantics, and `src/core/src/detail/posix_transport.cpp` owns the UDP transport. `netft::Client` is the public face of the snapshot: it fetches sensor calibration over HTTP unless an override is supplied, manages streaming and recovery, converts counts to the sensor's declared engineering units, invokes the sample callback, and exposes an immutable health snapshot. + +The snapshot deliberately stops at native engineering units. `netft::Sample` carries both scaled values and force/torque unit tags. `netft_driver::to_si_sample()` in `netft_ros_support` is the adapter boundary that converts those values to newtons and newton-metres. ATI RDT permits one active UDP client. The driver sends only Start Streaming, Stop Streaming, and an explicitly requested Software Bias command; it does @@ -53,19 +53,9 @@ settings. ## Acquisition and threading -`NetFTClient::start()` creates one receiver thread. That thread owns blocking -UDP receive activity, decodes complete datagrams, classifies sequence and -device state, updates the health snapshot, and invokes the adapter callback -for accepted samples. Shutdown closes the socket to wake blocked receive, -joins the worker, and sends Stop Streaming when possible. +`netft::Client::start()` creates one receiver thread. That thread owns blocking UDP receive activity, decodes complete datagrams, classifies sequence and device state, updates the health snapshot, and invokes the adapter callback for accepted samples. Shutdown closes the socket to wake blocked receive, joins the worker, and sends Stop Streaming when possible. -Standalone adapters publish from the receiver callback and keep ROS timers, -services, logging, and message conversion outside the core. The plugin's -receiver callback writes complete samples into -`realtime_tools::RealtimeBuffer`. Its controller-loop `read()` -uses `RealtimeBuffer::readFromRT()` and performs no network I/O, reconnect, -service work, or unbounded mutex wait. A controller loop may consume the same -current sample more than once when it runs faster than the sensor stream. +Standalone adapters convert each callback sample through the support layer before publishing and keep ROS timers, services, logging, and message construction outside the snapshot. The plugin's receiver callback also immediately converts each accepted native-unit sample to SI and writes the result into `realtime_tools::RealtimeBuffer`. Its controller-loop `read()` uses `RealtimeBuffer::readFromRT()`, copies SI values only, and performs no unit conversion, network I/O, reconnect, service work, or unbounded mutex wait. A controller loop may consume the same current sample more than once when it runs faster than the sensor stream. This separation is control-loop-friendly, but it is not an end-to-end hard-real-time guarantee: UDP transport, Linux scheduling, controller code, @@ -187,22 +177,18 @@ The package uses one format-3 manifest with ROS-version conditions. ROS 1 uses catkin and roscpp. ROS 2 uses ament_cmake, rclcpp, hardware_interface, pluginlib, and realtime_tools. -`include/netft_driver/ros2_control_compat.hpp` isolates the ros2_control API -difference. Hardware-interface major versions below 4 use the legacy exported -state-interface path required by Humble; version 4 and later use -framework-managed state interfaces. CMake selects this at compile time from -the discovered `hardware_interface` version. Transport and fault semantics do -not branch on a ROS distribution name. +The private `src/ros/ros2_control_compat.hpp` isolates the ros2_control API +difference, while private test-access declarations stay in +`src/ros/ros2_control_test_access.hpp`; neither header is installed. Hardware- +interface major versions below 4 use the legacy exported state-interface path +required by Humble; version 4 and later use framework-managed state interfaces. +CMake selects this at compile time from the discovered `hardware_interface` +version. Transport and fault semantics do not branch on a ROS distribution +name. ## Test boundaries -The ROS-neutral CTest suite builds `netft_core` without ROS and uses GTest for -protocol bytes, record parsing, status and sequence semantics, diagnostics, -parameter validation, client recovery, shutdown, and the bounded check tool. -ROS package GTests cover the native ROS adapters and the hardware plugin, -including invalid descriptions, activation, non-blocking reads, every fatal -class, persistent latching, lifecycle recovery, online bias, and auxiliary -thread cleanup. +The ROS-neutral CTest suite builds `netft_core` and `netft_ros_support` without ROS and uses GTest for protocol bytes, XML calibration, record parsing, status and sequence semantics, SI conversion, parameter validation, client recovery, shutdown, and the bounded check tool. ROS package GTests cover the native ROS adapters and load the non-installed instrumented hardware plugin, including invalid descriptions, activation, non-blocking reads, every fatal class, persistent latching, lifecycle quiescence and recovery, online bias, and auxiliary-thread cleanup. Installed-package smoke tests load only the production plugin. Loopback graph smoke tests build and use the installed package. They verify standalone topics, services, diagnostics, shutdown, plugin loading, the diff --git a/include/netft_driver/client.hpp b/include/netft_driver/client.hpp deleted file mode 100644 index b5a3f32..0000000 --- a/include/netft_driver/client.hpp +++ /dev/null @@ -1,97 +0,0 @@ -#pragma once - -#include "netft_driver/types.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace netft_driver { - -struct NetFTClientTestAccess; - -class NotConnectedError : public std::runtime_error { -public: - using std::runtime_error::runtime_error; -}; - -class NetFTClient { -public: - /** - * The client object must outlive every callback invocation. In particular, - * destroying the last owner from inside a callback is unsupported: C++17 - * cannot join the currently executing thread without deadlock or detach. - */ - using SampleCallback = std::function; - enum class SessionResult { Stopped, Timeout, Socket, SeriousStatus, FtStall, FtBackward, MalformedStorm, Callback }; - explicit NetFTClient(ClientConfig config); - ~NetFTClient(); - NetFTClient(const NetFTClient &) = delete; - NetFTClient & operator=(const NetFTClient &) = delete; - - void start(SampleCallback callback); - void stop() noexcept; - void bias(); - bool wait_for_first_sample(std::chrono::duration timeout); - bool faulted() const noexcept { return fault_code_.load(std::memory_order_acquire) != FaultCode::None; } - FaultCode fault_code() const noexcept { return fault_code_.load(std::memory_order_acquire); } - HealthSnapshot health_snapshot() const; - std::optional latest_sample() const; - -private: - using ThreadFactory = std::thread (*)(NetFTClient *); - - void run(); - std::thread create_worker_thread(); - SessionResult receive_session(); - std::optional handle_record(const struct RawRecord & record, - std::chrono::steady_clock::time_point received); - void latch_fault(FaultCode code, const char * message) noexcept; - void set_state(ClientState state); - void set_error(const char * message); - void record_callback_error(const char * message) noexcept; - void close_session() noexcept; - void send_command(std::uint16_t command); - - friend struct NetFTClientTestAccess; - - ClientConfig config_; - mutable std::mutex lifecycle_mutex_, health_mutex_, socket_mutex_, record_mutex_; - std::condition_variable lifecycle_cv_, first_sample_cv_; - std::thread worker_; - ThreadFactory thread_factory_{nullptr}; - std::thread::id active_worker_id_{}; - bool joining_{false}; - SampleCallback callback_; - int socket_{-1}; - bool session_started_{false}; - sockaddr_storage endpoint_{}; - socklen_t endpoint_size_{}; - bool endpoint_ready_{false}; - std::atomic stopping_{false}; - std::atomic worker_exited_{true}; - std::atomic recovered_{false}; - std::atomic fault_code_{FaultCode::None}; - HealthSnapshot health_; - std::optional latest_; - std::chrono::steady_clock::time_point last_publish_{}; - std::chrono::steady_clock::time_point last_valid_{}; - std::chrono::steady_clock::time_point session_started_at_{}; - std::uint64_t generation_{}, published_generation_{}; - mutable std::deque receive_times_, publish_times_; - std::unique_ptr rdt_; - std::unique_ptr ft_; - std::uint64_t consecutive_malformed_{}; -}; - -static_assert(std::atomic::is_always_lock_free, "fault code must be lock-free"); - -} // namespace netft_driver diff --git a/include/netft_driver/status.hpp b/include/netft_driver/status.hpp deleted file mode 100644 index 0fe3133..0000000 --- a/include/netft_driver/status.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "netft_driver/types.hpp" - -namespace netft_driver { - -enum class DiagnosticSeverity : int { Ok = 0, Warn = 1, Error = 2 }; -DiagnosticSeverity classify_status(std::uint32_t status); -std::string decode_status(std::uint32_t status); - -enum class SequenceKind { First, Contiguous, Gap, Duplicate, OutOfOrder }; -struct SequenceObservation { SequenceKind kind; std::uint32_t gap{0}; }; -class RdtSequenceTracker { -public: - SequenceObservation observe(std::uint32_t current); - void reset(); - bool has_last() const { return has_last_; } - std::uint32_t last() const { return previous_; } -private: - bool has_last_{false}; std::uint32_t previous_{}; -}; - -enum class FtSequenceKind { First, Forward, Stall, Backward, Restart }; -struct FtSequenceObservation { FtSequenceKind kind; }; -class FtSequenceTracker { -public: - FtSequenceObservation observe(std::uint32_t current); - void begin_session(); - bool has_last() const { return has_last_; } - std::uint32_t last() const { return previous_; } -private: - bool has_last_{false}; std::uint32_t previous_{}; - bool has_restart_candidate_{false}; std::uint32_t restart_candidate_{}; -}; - -inline constexpr std::uint64_t kMalformedStormThreshold = 10; -inline const std::vector kDiagnosticValueKeys{ - "state", "sensor", "last_rdt_sequence", "last_ft_sequence", "last_ft_progress", "device_status", "active_status", "receive_rate_hz", "expected_receive_rate_hz", "rate_tolerance", "publish_rate_hz", "received_count", "published_count", "rate_dropped_count", "device_error_count", "lost_count", "duplicate_count", "out_of_order_count", "ft_stall_count", "ft_backward_count", "ft_restart_count", "malformed_count", "malformed_storm_threshold", "malformed_storm_window", "reconnect_count", "timeout_count", "callback_error_count", "last_record_age_s", "last_error"}; - -struct DiagnosticReport { - int level{}; std::string message; std::vector> values; std::string log_key; -}; -class DiagnosticEvaluator { -public: - DiagnosticEvaluator(double expected_rdt_rate, double rate_tolerance); - DiagnosticEvaluator(double expected_rdt_rate, bool rate_tolerance) = delete; - DiagnosticReport evaluate(const HealthSnapshot & snapshot); -private: - double expected_rdt_rate_; double rate_tolerance_; - std::uint64_t lost_{}, device_error_{}, timeout_{}, malformed_{}, ft_stall_{}, ft_backward_{}, ft_restart_{}; -}; -class FaultLogThrottle { -public: - explicit FaultLogThrottle(double repeat_interval = 10.0); - bool should_log(const DiagnosticReport & report, double now); -private: - double repeat_interval_; bool active_{false}; int level_{}; std::string key_; double last_log_{}; -}; - -} // namespace netft_driver diff --git a/include/netft_driver/types.hpp b/include/netft_driver/types.hpp deleted file mode 100644 index dfd000f..0000000 --- a/include/netft_driver/types.hpp +++ /dev/null @@ -1,97 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace netft_driver { - -enum class RecoveryPolicy { Reconnect, FailStop }; -enum class ClientState { Stopped, Connecting, Streaming, Backoff, Faulted }; -enum class FaultCode { None, Timeout, Socket, SeriousStatus, FtStall, - FtBackward, MalformedStorm, Callback }; - -struct ClientConfig { - std::string sensor_host{"192.168.1.1"}; - int sensor_port{49152}; - double counts_per_force{1000000.0}; - double counts_per_torque{1000000.0}; - double publish_rate{0.0}; - std::chrono::duration receive_timeout{0.1}; - std::chrono::duration reconnect_initial_delay{0.25}; - std::chrono::duration reconnect_max_delay{5.0}; - bool publish_on_error{false}; - RecoveryPolicy recovery_policy{RecoveryPolicy::Reconnect}; -}; - -struct WrenchSample { - std::uint32_t rdt_sequence{}, ft_sequence{}, status{}; - std::array force{}, torque{}; - std::chrono::steady_clock::time_point received_at{}; -}; - -struct HealthSnapshot { - ClientState state{ClientState::Stopped}; - FaultCode fault_code{FaultCode::None}; - std::string sensor_host{}; - int sensor_port{}; - std::optional last_rdt_sequence{}; - std::optional last_ft_sequence{}; - std::uint32_t last_status{}; - double receive_rate{}; - double publish_rate{}; - std::uint64_t received_count{}; - std::uint64_t published_count{}; - std::uint64_t rate_dropped_count{}; - std::uint64_t device_error_count{}; - std::uint64_t warning_count{}; - std::uint64_t lost_count{}; - std::uint64_t duplicate_count{}; - std::uint64_t out_of_order_count{}; - std::uint64_t malformed_count{}; - std::uint64_t reconnect_count{}; - std::uint64_t timeout_count{}; - std::uint64_t callback_error_count{}; - std::optional> last_record_age{}; - std::string last_error{}; - std::uint64_t ft_stall_count{}; - std::uint64_t ft_backward_count{}; - std::uint64_t ft_restart_count{}; - std::string last_ft_progress{"unknown"}; -}; - -inline void validate(const ClientConfig & config) -{ - if (config.sensor_host.find_first_not_of(" \t\n\r\f\v") == std::string::npos) { - throw std::invalid_argument{"sensor_host must be non-empty"}; - } - if (config.sensor_port < 1 || config.sensor_port > 65535) { - throw std::invalid_argument{"sensor_port must be between 1 and 65535"}; - } - - const auto positive_finite = [](const char * field, double value) { - if (!std::isfinite(value) || value <= 0.0) { - throw std::invalid_argument{std::string{field} + " must be finite and greater than zero"}; - } - }; - positive_finite("counts_per_force", config.counts_per_force); - positive_finite("counts_per_torque", config.counts_per_torque); - - if (!std::isfinite(config.publish_rate) || config.publish_rate < 0.0) { - throw std::invalid_argument{"publish_rate must be finite and non-negative"}; - } - - positive_finite("receive_timeout", config.receive_timeout.count()); - positive_finite("reconnect_initial_delay", config.reconnect_initial_delay.count()); - positive_finite("reconnect_max_delay", config.reconnect_max_delay.count()); - if (config.reconnect_max_delay < config.reconnect_initial_delay) { - throw std::invalid_argument{ - "reconnect_max_delay must be at least reconnect_initial_delay"}; - } -} - -} // namespace netft_driver diff --git a/launch/netft.launch b/launch/netft.launch index 66dd0ec..4690675 100644 --- a/launch/netft.launch +++ b/launch/netft.launch @@ -1,9 +1,17 @@ + + + + - + + + + + diff --git a/launch/netft.launch.py b/launch/netft.launch.py index b592d0f..2295010 100644 --- a/launch/netft.launch.py +++ b/launch/netft.launch.py @@ -15,6 +15,12 @@ def generate_launch_description(): [ DeclareLaunchArgument("sensor_ip", default_value="192.168.1.1"), DeclareLaunchArgument("sensor_port", default_value="49152"), + DeclareLaunchArgument("http_port", default_value="80"), + DeclareLaunchArgument("use_sensor_calibration", default_value="true"), + DeclareLaunchArgument( + "configuration_connect_timeout", default_value="0.5" + ), + DeclareLaunchArgument("configuration_timeout", default_value="1.0"), Node( package="netft_driver", executable="netft_node", @@ -27,6 +33,21 @@ def generate_launch_description(): "sensor_port": ParameterValue( LaunchConfiguration("sensor_port"), value_type=int ), + "http_port": ParameterValue( + LaunchConfiguration("http_port"), value_type=int + ), + "use_sensor_calibration": ParameterValue( + LaunchConfiguration("use_sensor_calibration"), + value_type=bool, + ), + "configuration_connect_timeout": ParameterValue( + LaunchConfiguration("configuration_connect_timeout"), + value_type=float, + ), + "configuration_timeout": ParameterValue( + LaunchConfiguration("configuration_timeout"), + value_type=float, + ), }, ], ), diff --git a/launch/netft_ros2_control.launch.py b/launch/netft_ros2_control.launch.py index ae2de60..2a94cab 100644 --- a/launch/netft_ros2_control.launch.py +++ b/launch/netft_ros2_control.launch.py @@ -13,7 +13,19 @@ def _nodes(context): controller_config = f"{share}/config/netft_ros2_control.yaml" values = { name: LaunchConfiguration(name).perform(context) - for name in ("sensor_name", "sensor_ip", "sensor_port") + for name in ( + "sensor_name", + "sensor_ip", + "sensor_port", + "http_port", + "use_sensor_calibration", + "counts_per_force", + "counts_per_torque", + "receive_timeout", + "configuration_connect_timeout", + "configuration_timeout", + "activation_timeout", + ) } wrapper = f""" @@ -21,7 +33,14 @@ def _nodes(context): + sensor_port='{values['sensor_port']}' http_port='{values['http_port']}' + use_sensor_calibration='{values['use_sensor_calibration']}' + counts_per_force='{values['counts_per_force']}' + counts_per_torque='{values['counts_per_torque']}' + receive_timeout='{values['receive_timeout']}' + configuration_connect_timeout='{values['configuration_connect_timeout']}' + configuration_timeout='{values['configuration_timeout']}' + activation_timeout='{values['activation_timeout']}'/> """ document = minidom.parseString(wrapper) xacro.process_doc(document) @@ -60,6 +79,16 @@ def generate_launch_description(): DeclareLaunchArgument("sensor_name", default_value="netft_sensor"), DeclareLaunchArgument("sensor_ip", default_value="192.168.1.1"), DeclareLaunchArgument("sensor_port", default_value="49152"), + DeclareLaunchArgument("http_port", default_value="80"), + DeclareLaunchArgument("use_sensor_calibration", default_value="true"), + DeclareLaunchArgument("counts_per_force", default_value="1000000"), + DeclareLaunchArgument("counts_per_torque", default_value="1000000"), + DeclareLaunchArgument("receive_timeout", default_value="0.1"), + DeclareLaunchArgument( + "configuration_connect_timeout", default_value="0.5" + ), + DeclareLaunchArgument("configuration_timeout", default_value="1.0"), + DeclareLaunchArgument("activation_timeout", default_value="2.0"), OpaqueFunction(function=_nodes), ] ) diff --git a/package.xml b/package.xml index 5a74c8e..36aa3ab 100644 --- a/package.xml +++ b/package.xml @@ -2,10 +2,10 @@ netft_driver - 0.2.2 + 0.3.0 Native ROS 1, ROS 2, and ros2_control driver for ATI Net F/T RDT sensors. Xudong Han - MIT + Apache-2.0 https://github.com/netft/ros-netft https://github.com/netft/ros-netft https://github.com/netft/ros-netft/issues @@ -22,6 +22,7 @@ geometry_msgs std_srvs diagnostic_msgs + libcurl-dev roslaunch roscpp diff --git a/pixi.lock b/pixi.lock index a0f4770..bda48c2 100644 --- a/pixi.lock +++ b/pixi.lock @@ -14,14 +14,26 @@ environments: linux-64: - conda: https://prefix.dev/conda-forge/linux-64/_openmp_mutex-4.5-20_gnu.conda - conda: https://prefix.dev/conda-forge/linux-64/bzip2-1.0.8-hda65f42_9.conda + - conda: https://prefix.dev/conda-forge/linux-64/c-ares-1.34.8-hb03c661_0.conda + - conda: https://prefix.dev/conda-forge/linux-64/icu-78.3-h33c6efd_1.conda + - conda: https://prefix.dev/conda-forge/linux-64/keyutils-1.6.3-hb9d3cd8_0.conda + - conda: https://prefix.dev/conda-forge/linux-64/krb5-1.22.2-hbde042b_1.conda - conda: https://prefix.dev/conda-forge/linux-64/ld_impl_linux-64-2.46.1-default_hbd61a6d_102.conda + - conda: https://prefix.dev/conda-forge/linux-64/libcurl-8.21.0-hae6b9f4_2.conda + - conda: https://prefix.dev/conda-forge/linux-64/libedit-3.1.20250104-pl5321h7949ede_0.conda + - conda: https://prefix.dev/conda-forge/linux-64/libev-4.33-hd590300_2.conda - conda: https://prefix.dev/conda-forge/linux-64/libexpat-2.8.1-hecca717_1.conda - conda: https://prefix.dev/conda-forge/linux-64/libffi-3.5.2-h3435931_0.conda - conda: https://prefix.dev/conda-forge/linux-64/libgcc-15.2.0-he0feb66_19.conda + - conda: https://prefix.dev/conda-forge/linux-64/libgcc-ng-15.2.0-h69a702a_19.conda - conda: https://prefix.dev/conda-forge/linux-64/libgomp-15.2.0-he0feb66_19.conda - conda: https://prefix.dev/conda-forge/linux-64/liblzma-5.8.3-hb03c661_0.conda - conda: https://prefix.dev/conda-forge/linux-64/libmpdec-4.0.0-hb03c661_1.conda + - conda: https://prefix.dev/conda-forge/linux-64/libnghttp2-1.68.1-h877daf1_0.conda + - conda: https://prefix.dev/conda-forge/linux-64/libpsl-0.22.0-h49b2146_1.conda - conda: https://prefix.dev/conda-forge/linux-64/libsqlite-3.53.3-h0c1763c_0.conda + - conda: https://prefix.dev/conda-forge/linux-64/libssh2-1.11.1-hcf80075_0.conda + - conda: https://prefix.dev/conda-forge/linux-64/libstdcxx-15.2.0-h934c35e_19.conda - conda: https://prefix.dev/conda-forge/linux-64/libuuid-2.42.2-h5347b49_0.conda - conda: https://prefix.dev/conda-forge/linux-64/libzlib-1.3.2-h25fd6f3_2.conda - conda: https://prefix.dev/conda-forge/linux-64/ncurses-6.6-hdb14827_0.conda @@ -4416,6 +4428,20 @@ packages: - icu >=78.3,<79.0a0 size: 12723451 timestamp: 1773822285671 +- conda: https://prefix.dev/conda-forge/linux-64/icu-78.3-h33c6efd_1.conda + sha256: c054e32ac4c14426006edf9d01981dc564559b4065af7c13c26be904e1babf04 + md5: 3c7763347873f07adfd87e8001b5b1d1 + depends: + - __glibc >=2.17,<3.0.a0 + - libgcc >=14 + - libstdcxx >=14 + license: MIT + license_family: MIT + run_exports: + weak: + - icu >=78.3,<79.0a0 + size: 12709032 + timestamp: 1784588496729 - conda: https://prefix.dev/conda-forge/linux-64/keyutils-1.6.3-hb9d3cd8_0.conda sha256: 0960d06048a7185d3542d850986d807c6e37ca2e644342dd0c72feefcf26c2a4 md5: b38117a3c920364aff79f870c984b4a3 diff --git a/pixi.toml b/pixi.toml index a0fcec5..a205c05 100644 --- a/pixi.toml +++ b/pixi.toml @@ -2,13 +2,14 @@ [workspace] name = "ros-netft" -version = "0.2.2" +version = "0.3.0" description = "Native ROS 1, ROS 2, and ros2_control driver for ATI Net F/T RDT sensors" channels = ["https://prefix.dev/conda-forge"] platforms = ["linux-64"] channel-priority = "strict" [dependencies] +libcurl = ">=7.63" python = ">=3.8" pytest = ">=8,<9" pytest-timeout = ">=2,<3" diff --git a/src/check_main.cpp b/src/check_main.cpp index e6a09f5..2fcbe4b 100644 --- a/src/check_main.cpp +++ b/src/check_main.cpp @@ -1,4 +1,5 @@ -#include "netft_driver/client.hpp" +#include "netft/client.hpp" +#include "ros/unit_conversion.hpp" #include #include @@ -23,10 +24,10 @@ #include namespace { -using netft_driver::ClientConfig; -using netft_driver::HealthSnapshot; -using netft_driver::NetFTClient; -using netft_driver::WrenchSample; +using netft::Client; +using netft::Config; +using netft::HealthSnapshot; +using netft::Sample; volatile std::sig_atomic_t interrupted = 0; void handle_interrupt(int) { interrupted = 1; } @@ -42,11 +43,11 @@ class ScopeExit { class LastSampleSlot { public: - void store(const WrenchSample & sample) { + void store(const Sample & sample) { { std::lock_guard lock(mutex_); sample_ = sample; } changed_.notify_all(); } - std::optional load() const { + std::optional load() const { std::lock_guard lock(mutex_); return sample_; } void wait_for_change(std::chrono::milliseconds timeout) { @@ -56,11 +57,11 @@ class LastSampleSlot { private: mutable std::mutex mutex_; std::condition_variable changed_; - std::optional sample_; + std::optional sample_; }; struct Options { - ClientConfig config; + Config config; double duration{5.0}; std::optional output; bool help{false}; @@ -71,11 +72,13 @@ struct Result { double requested_duration_s{}; double elapsed_s{}; HealthSnapshot health; - std::optional last; + std::optional last; }; const char * usage() { return "usage: netft_check [--host HOST] [--port PORT] [--duration SECONDS] " + "[--http-port PORT] [--configuration-connect-timeout SECONDS] " + "[--configuration-timeout SECONDS] " "[--counts-per-force SCALE] [--counts-per-torque SCALE] " "[--receive-timeout SECONDS] [--output PATH] [--json]"; } @@ -102,6 +105,8 @@ int integer(const std::string & field, const std::string & value) { Options parse_options(int argc, char ** argv) { Options options; + std::optional counts_per_force; + std::optional counts_per_torque; for (int index = 1; index < argc; ++index) { std::string argument{argv[index]}; if (argument == "--json") continue; @@ -109,7 +114,9 @@ Options parse_options(int argc, char ** argv) { const auto equals = argument.find('='); const auto key = argument.substr(0, equals); std::string value; - if (key == "--host" || key == "--port" || key == "--duration" || + if (key == "--host" || key == "--port" || key == "--http-port" || + key == "--configuration-connect-timeout" || key == "--configuration-timeout" || + key == "--duration" || key == "--counts-per-force" || key == "--counts-per-torque" || key == "--receive-timeout" || key == "--output") { if (equals != std::string::npos) value = argument.substr(equals + 1); @@ -119,29 +126,51 @@ Options parse_options(int argc, char ** argv) { throw std::invalid_argument{"unknown argument: " + argument}; } if (key == "--host") options.config.sensor_host = value; - else if (key == "--port") options.config.sensor_port = integer("port", value); + else if (key == "--port") options.config.rdt_port = integer("port", value); + else if (key == "--http-port") options.config.http_port = integer("http-port", value); + else if (key == "--configuration-connect-timeout") { + options.config.configuration_connect_timeout = + std::chrono::duration{number("configuration-connect-timeout", value)}; + } + else if (key == "--configuration-timeout") { + options.config.configuration_timeout = + std::chrono::duration{number("configuration-timeout", value)}; + } else if (key == "--duration") options.duration = number("duration", value); - else if (key == "--counts-per-force") options.config.counts_per_force = number("counts-per-force", value); - else if (key == "--counts-per-torque") options.config.counts_per_torque = number("counts-per-torque", value); + else if (key == "--counts-per-force") { + counts_per_force = number("counts-per-force", value); + } + else if (key == "--counts-per-torque") { + counts_per_torque = number("counts-per-torque", value); + } else if (key == "--receive-timeout") options.config.receive_timeout = std::chrono::duration{number("receive-timeout", value)}; else if (key == "--output") options.output = value; } if (!std::isfinite(options.duration) || options.duration <= 0.0) { throw std::invalid_argument{"duration must be finite and greater than zero"}; } - netft_driver::validate(options.config); + if (counts_per_force.has_value() != counts_per_torque.has_value()) { + throw std::invalid_argument{"counts-per-force and counts-per-torque must be provided together"}; + } + if (counts_per_force) { + options.config.calibration_override = netft::Calibration{ + *counts_per_force, *counts_per_torque, + netft::ForceUnit::Newton, netft::TorqueUnit::NewtonMeter}; + } + netft::validate(options.config); return options; } Result run_check(const Options & options) { LastSampleSlot latest; - NetFTClient client{options.config}; + Client client{options.config}; const auto started = std::chrono::steady_clock::now(); ScopeExit stop{[&] { client.stop(); }}; - client.start([&](const WrenchSample & sample) { latest.store(sample); }); + client.start([&](const Sample & sample) { latest.store(sample); }); const auto first_deadline = started + std::chrono::duration{std::max(1.0, options.config.receive_timeout.count() * 5.0)}; - while (client.health_snapshot().received_count == 0) { + while (client.health().received_count == 0) { if (interrupted) throw std::runtime_error{"interrupted"}; + if (client.faulted()) throw std::runtime_error{client.health().last_error}; if (std::chrono::steady_clock::now() >= first_deadline) { throw std::runtime_error{"no Net F/T sample received"}; } @@ -154,8 +183,8 @@ Result run_check(const Options & options) { } client.stop(); const auto elapsed = std::chrono::duration{std::chrono::steady_clock::now() - started}.count(); - return {options.config.sensor_host + ":" + std::to_string(options.config.sensor_port), options.duration, - elapsed, client.health_snapshot(), latest.load()}; + return {options.config.sensor_host + ":" + std::to_string(options.config.rdt_port), options.duration, + elapsed, client.health(), latest.load()}; } std::string json_string(const std::string & value) { @@ -185,25 +214,48 @@ std::string json_optional_sequence(const std::optional & value) { return value ? std::to_string(*value) : "null"; } -std::string json_wrench(const std::optional & sample, bool force) { +std::string json_wrench(const std::optional & sample, bool force) { if (!sample) return "null"; const auto & values = force ? sample->force : sample->torque; return "[" + json_number(values[0]) + ", " + json_number(values[1]) + ", " + json_number(values[2]) + "]"; } +std::string configuration_source(const netft::CalibrationSource source) { + return source == netft::CalibrationSource::Sensor ? "sensor" : "override"; +} + std::string serialize(const Result & result) { const auto & health = result.health; + const auto & configuration = health.sensor_configuration; + const auto si_sample = result.last + ? std::optional{netft_driver::to_si_sample(*result.last)} + : std::nullopt; std::ostringstream output; output << "{\n" + << " \"configuration_source\": " + << (configuration ? json_string(configuration_source(configuration->source)) : "null") + << ",\n" + << " \"force_unit\": " + << (configuration ? json_string(std::string{netft::to_string( + configuration->calibration.force_unit)}) : "null") << ",\n" + << " \"torque_unit\": " + << (configuration ? json_string(std::string{netft::to_string( + configuration->calibration.torque_unit)}) : "null") << ",\n" + << " \"counts_per_force_unit\": " + << (configuration ? json_number(configuration->calibration.counts_per_force_unit) : "null") + << ",\n" + << " \"counts_per_torque_unit\": " + << (configuration ? json_number(configuration->calibration.counts_per_torque_unit) : "null") + << ",\n" << " \"device_error_count\": " << health.device_error_count << ",\n" << " \"device_status\": \"0x" << std::hex << std::setw(8) << std::setfill('0') << health.last_status << std::dec << "\",\n" << " \"duplicate_count\": " << health.duplicate_count << ",\n" << " \"elapsed_s\": " << json_number(result.elapsed_s) << ",\n" << " \"endpoint\": " << json_string(result.endpoint) << ",\n" - << " \"last_force\": " << json_wrench(result.last, true) << ",\n" + << " \"last_force_n\": " << json_wrench(si_sample, true) << ",\n" << " \"last_ft_sequence\": " << json_optional_sequence(health.last_ft_sequence) << ",\n" << " \"last_rdt_sequence\": " << json_optional_sequence(health.last_rdt_sequence) << ",\n" - << " \"last_torque\": " << json_wrench(result.last, false) << ",\n" + << " \"last_torque_nm\": " << json_wrench(si_sample, false) << ",\n" << " \"lost_count\": " << health.lost_count << ",\n" << " \"malformed_count\": " << health.malformed_count << ",\n" << " \"out_of_order_count\": " << health.out_of_order_count << ",\n" @@ -216,6 +268,11 @@ std::string serialize(const Result & result) { return output.str(); } +std::string serialize_error(const std::string & category, const std::string & message) { + return "{\n \"error\": " + json_string(category) + + ",\n \"exit_code\": 2,\n \"message\": " + json_string(message) + "\n}"; +} + void write_output(const std::string & path, const std::string & text) { const std::filesystem::path output{path}; const auto parent = output.parent_path().empty() ? std::filesystem::path{"."} : output.parent_path(); @@ -250,8 +307,12 @@ int main(int argc, char ** argv) { return result.health.received_count > 0 && result.health.device_error_count == 0 && result.health.warning_count == 0 ? 0 : 1; } + catch (const std::invalid_argument & error) { + std::cerr << serialize_error("invalid_arguments", error.what()) << '\n'; + return 2; + } catch (const std::exception & error) { - std::cerr << "Net F/T check failed: " << error.what() << '\n'; + std::cerr << serialize_error("check_failed", error.what()) << '\n'; return 2; } } diff --git a/src/client.cpp b/src/client.cpp deleted file mode 100644 index 0caebca..0000000 --- a/src/client.cpp +++ /dev/null @@ -1,388 +0,0 @@ -#include "netft_driver/client.hpp" - -#include "netft_driver/protocol.hpp" -#include "netft_driver/status.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace netft_driver { -namespace { -FaultCode fault_for(NetFTClient::SessionResult result) -{ - switch (result) { - case NetFTClient::SessionResult::Timeout: return FaultCode::Timeout; - case NetFTClient::SessionResult::Socket: return FaultCode::Socket; - case NetFTClient::SessionResult::SeriousStatus: return FaultCode::SeriousStatus; - case NetFTClient::SessionResult::FtStall: return FaultCode::FtStall; - case NetFTClient::SessionResult::FtBackward: return FaultCode::FtBackward; - case NetFTClient::SessionResult::MalformedStorm: return FaultCode::MalformedStorm; - case NetFTClient::SessionResult::Callback: return FaultCode::Callback; - case NetFTClient::SessionResult::Stopped: return FaultCode::None; - } - return FaultCode::Socket; -} -const char * message_for(NetFTClient::SessionResult result) -{ - switch (result) { - case NetFTClient::SessionResult::Timeout: return "no valid RDT record before timeout"; - case NetFTClient::SessionResult::Socket: return "UDP socket failure"; - case NetFTClient::SessionResult::SeriousStatus: return "serious device status"; - case NetFTClient::SessionResult::FtStall: return "FT sequence stalled"; - case NetFTClient::SessionResult::FtBackward: return "FT sequence moved backward"; - case NetFTClient::SessionResult::MalformedStorm: return "malformed packet storm"; - case NetFTClient::SessionResult::Callback: return "sample callback failed"; - case NetFTClient::SessionResult::Stopped: return ""; - } - return "UDP socket failure"; -} -} - -NetFTClient::NetFTClient(ClientConfig config) : config_(std::move(config)) -{ - validate(config_); - rdt_ = std::make_unique(); - ft_ = std::make_unique(); - health_.sensor_host = config_.sensor_host; health_.sensor_port = config_.sensor_port; - addrinfo hints{}; hints.ai_family = AF_INET; hints.ai_socktype = SOCK_DGRAM; - addrinfo * result = nullptr; - if (::getaddrinfo(config_.sensor_host.c_str(), std::to_string(config_.sensor_port).c_str(), &hints, &result) == 0) { - std::memcpy(&endpoint_, result->ai_addr, result->ai_addrlen); - endpoint_size_ = static_cast(result->ai_addrlen); - endpoint_ready_ = true; - ::freeaddrinfo(result); - } -} -NetFTClient::~NetFTClient() { stop(); } - -void NetFTClient::start(SampleCallback callback) -{ - for (;;) { - std::thread reaped_worker; - { - std::unique_lock lock(lifecycle_mutex_); - if (!worker_exited_.load(std::memory_order_acquire) && active_worker_id_ == std::this_thread::get_id()) throw std::runtime_error{"client is already running"}; - lifecycle_cv_.wait(lock, [&] { return !joining_; }); - if (worker_.joinable()) { - if (!worker_exited_.load(std::memory_order_acquire)) throw std::runtime_error{"client is already running"}; - joining_ = true; - reaped_worker = std::move(worker_); - } else { - const auto previous_stopping = stopping_.load(std::memory_order_relaxed); - const auto previous_worker_exited = worker_exited_.load(std::memory_order_relaxed); - const auto previous_fault_code = fault_code_.load(std::memory_order_relaxed); - auto previous_callback = std::move(callback_); - std::chrono::steady_clock::time_point previous_last_publish; - { - std::lock_guard record_lock(record_mutex_); - previous_last_publish = last_publish_; - last_publish_ = {}; - } - ClientState previous_state; - FaultCode previous_health_fault_code; - std::string previous_last_error; - std::optional previous_latest; - std::chrono::steady_clock::time_point previous_last_valid; - std::chrono::steady_clock::time_point previous_session_started_at; - std::uint64_t previous_generation; - std::uint64_t previous_published_generation; - std::deque previous_receive_times; - std::deque previous_publish_times; - double previous_receive_rate; - double previous_publish_rate; - { - std::lock_guard health_lock(health_mutex_); - previous_state = health_.state; - previous_health_fault_code = health_.fault_code; - previous_last_error = std::move(health_.last_error); - previous_latest = std::move(latest_); - previous_last_valid = last_valid_; - previous_session_started_at = session_started_at_; - previous_generation = generation_; - previous_published_generation = published_generation_; - previous_receive_times = std::move(receive_times_); - previous_publish_times = std::move(publish_times_); - previous_receive_rate = health_.receive_rate; - previous_publish_rate = health_.publish_rate; - - fault_code_.store(FaultCode::None, std::memory_order_release); - health_.state = ClientState::Connecting; - health_.fault_code = FaultCode::None; - health_.last_error.clear(); - latest_.reset(); - last_valid_ = {}; - session_started_at_ = std::chrono::steady_clock::now(); - ++generation_; - published_generation_ = 0; - receive_times_.clear(); - publish_times_.clear(); - health_.receive_rate = 0; - health_.publish_rate = 0; - } - stopping_ = false; - worker_exited_ = false; - callback_ = std::move(callback); - try { - worker_ = create_worker_thread(); - } - catch (...) { - stopping_.store(previous_stopping, std::memory_order_relaxed); - worker_exited_.store(previous_worker_exited, std::memory_order_relaxed); - fault_code_.store(previous_fault_code, std::memory_order_relaxed); - callback_ = std::move(previous_callback); - { - std::lock_guard record_lock(record_mutex_); - last_publish_ = previous_last_publish; - } - { - std::lock_guard health_lock(health_mutex_); - health_.state = previous_state; - health_.fault_code = previous_health_fault_code; - health_.last_error = std::move(previous_last_error); - latest_ = std::move(previous_latest); - last_valid_ = previous_last_valid; - session_started_at_ = previous_session_started_at; - generation_ = previous_generation; - published_generation_ = previous_published_generation; - receive_times_ = std::move(previous_receive_times); - publish_times_ = std::move(previous_publish_times); - health_.receive_rate = previous_receive_rate; - health_.publish_rate = previous_publish_rate; - } - first_sample_cv_.notify_all(); - throw; - } - active_worker_id_ = worker_.get_id(); - return; - } - } - reaped_worker.join(); - { std::lock_guard lock(lifecycle_mutex_); active_worker_id_ = {}; joining_ = false; } - lifecycle_cv_.notify_all(); - } -} -std::thread NetFTClient::create_worker_thread() -{ - if (thread_factory_) return thread_factory_(this); - return std::thread(&NetFTClient::run, this); -} -void NetFTClient::stop() noexcept -{ - std::thread joining_worker; - { - std::unique_lock lifecycle_lock(lifecycle_mutex_); - stopping_ = true; - first_sample_cv_.notify_all(); - { std::lock_guard lock(socket_mutex_); - if (socket_ >= 0) { - if (session_started_) { const auto bytes = encode_request(Command::StopStreaming); (void)::send(socket_, bytes.data(), bytes.size(), 0); session_started_ = false; } - ::shutdown(socket_, SHUT_RDWR); - } - } - if (!worker_exited_.load(std::memory_order_acquire) && active_worker_id_ == std::this_thread::get_id()) return; - if (joining_) { lifecycle_cv_.wait(lifecycle_lock, [&] { return !joining_; }); return; } - if (!worker_.joinable()) { set_state(ClientState::Stopped); return; } - joining_ = true; - joining_worker = std::move(worker_); - } - joining_worker.join(); - { - std::lock_guard lifecycle_lock(lifecycle_mutex_); - set_state(ClientState::Stopped); - active_worker_id_ = {}; - joining_ = false; - } - lifecycle_cv_.notify_all(); -} -void NetFTClient::bias() -{ - { std::lock_guard lock(health_mutex_); if (health_.state != ClientState::Streaming) throw NotConnectedError{"client is not streaming"}; } - std::lock_guard record_lock(record_mutex_); - send_command(static_cast(Command::SetSoftwareBias)); - send_command(static_cast(Command::StartRealtime)); - rdt_->reset(); -} -bool NetFTClient::wait_for_first_sample(std::chrono::duration timeout) -{ - std::unique_lock lock(health_mutex_); - const auto captured_generation = generation_; - if (captured_generation == 0) return false; - first_sample_cv_.wait_for(lock, timeout, [&] { return generation_ != captured_generation || published_generation_ == captured_generation || faulted() || stopping_; }); - if (generation_ != captured_generation) return false; - return published_generation_ == captured_generation; -} -HealthSnapshot NetFTClient::health_snapshot() const -{ - std::lock_guard lock(health_mutex_); const auto now = std::chrono::steady_clock::now(); - const auto prune = [&](auto & times) { while (!times.empty() && now - times.front() > std::chrono::seconds{1}) times.pop_front(); }; - prune(receive_times_); prune(publish_times_); - auto snapshot = health_; snapshot.receive_rate = static_cast(receive_times_.size()); snapshot.publish_rate = static_cast(publish_times_.size()); snapshot.fault_code = fault_code(); - if (snapshot.fault_code != FaultCode::None) snapshot.state = ClientState::Faulted; - if (last_valid_.time_since_epoch().count() != 0) snapshot.last_record_age = now - last_valid_; - return snapshot; -} -std::optional NetFTClient::latest_sample() const { std::lock_guard lock(health_mutex_); return latest_; } -void NetFTClient::set_state(ClientState state) { std::lock_guard lock(health_mutex_); health_.state = state; } -void NetFTClient::set_error(const char * message) { std::lock_guard lock(health_mutex_); health_.last_error = message; } -void NetFTClient::record_callback_error(const char * message) noexcept -{ - try { - std::lock_guard lock(health_mutex_); - ++health_.callback_error_count; - try { health_.last_error = message; } - catch (...) {} - } - catch (...) {} -} -void NetFTClient::latch_fault(FaultCode code, const char * message) noexcept -{ - FaultCode expected = FaultCode::None; - const bool first_fault = fault_code_.compare_exchange_strong(expected, code, std::memory_order_acq_rel); - try { - std::lock_guard lock(health_mutex_); - health_.state = ClientState::Faulted; - if (first_fault) { - try { health_.last_error = message; } - catch (...) {} - } - } - catch (...) {} - first_sample_cv_.notify_all(); -} -void NetFTClient::send_command(std::uint16_t command) -{ - const auto bytes = encode_request(static_cast(command)); - std::lock_guard lock(socket_mutex_); - if (socket_ < 0 || ::send(socket_, bytes.data(), bytes.size(), 0) != static_cast(bytes.size())) throw NotConnectedError{"client is not streaming"}; -} -void NetFTClient::close_session() noexcept -{ - std::lock_guard lock(socket_mutex_); - if (socket_ < 0) return; - if (session_started_) { const auto bytes = encode_request(Command::StopStreaming); (void)::send(socket_, bytes.data(), bytes.size(), 0); session_started_ = false; } - ::close(socket_); socket_ = -1; -} -void NetFTClient::run() -{ - auto backoff = config_.reconnect_initial_delay; - while (!stopping_ && !faulted()) { - const auto result = receive_session(); - close_session(); - if (result != SessionResult::Stopped && config_.recovery_policy == RecoveryPolicy::FailStop) { latch_fault(fault_for(result), message_for(result)); break; } - if (result == SessionResult::Stopped || stopping_) break; - if (recovered_.exchange(false, std::memory_order_acq_rel)) backoff = config_.reconnect_initial_delay; - set_error(message_for(result)); set_state(ClientState::Backoff); - { std::lock_guard lock(health_mutex_); ++health_.reconnect_count; } - const auto until = std::chrono::steady_clock::now() + backoff; - while (!stopping_ && std::chrono::steady_clock::now() < until) std::this_thread::sleep_for(std::chrono::milliseconds{1}); - backoff = std::min(backoff * 2, config_.reconnect_max_delay); - } - close_session(); - worker_exited_.store(true, std::memory_order_release); - if (!faulted()) set_state(ClientState::Stopped); - lifecycle_cv_.notify_all(); -} -NetFTClient::SessionResult NetFTClient::receive_session() -{ - set_state(ClientState::Connecting); - if (stopping_) return SessionResult::Stopped; - if (!endpoint_ready_) return stopping_ ? SessionResult::Stopped : SessionResult::Socket; - const int fd = ::socket(endpoint_.ss_family, SOCK_DGRAM, 0); - if (fd < 0 || ::connect(fd, reinterpret_cast(&endpoint_), endpoint_size_) != 0) { if (fd >= 0) ::close(fd); return stopping_ ? SessionResult::Stopped : SessionResult::Socket; } - { std::lock_guard lock(socket_mutex_); socket_ = fd; session_started_ = true; } - try { send_command(static_cast(Command::StartRealtime)); } catch (...) { return stopping_ ? SessionResult::Stopped : SessionResult::Socket; } - { std::lock_guard record_lock(record_mutex_); rdt_->reset(); ft_->begin_session(); } - const auto deadline_duration = config_.receive_timeout; - auto deadline = std::chrono::steady_clock::now() + deadline_duration; - std::array bytes{}; - while (!stopping_) { - const auto now = std::chrono::steady_clock::now(); - if (now >= deadline) { - std::lock_guard lock(health_mutex_); - if (stopping_) return SessionResult::Stopped; - ++health_.timeout_count; - return SessionResult::Timeout; - } - const auto remaining = std::chrono::duration_cast(deadline - now); - pollfd pollfd{fd, POLLIN, 0}; const int wait = std::max(1, std::min(50, static_cast(remaining.count()))); - const int polled = ::poll(&pollfd, 1, wait); - if (polled < 0) { if (errno == EINTR) continue; return stopping_ ? SessionResult::Stopped : SessionResult::Socket; } - if (polled == 0) continue; - if (stopping_) return SessionResult::Stopped; - if ((pollfd.revents & POLLIN) == 0) return stopping_ ? SessionResult::Stopped : SessionResult::Socket; - const auto size = ::recv(fd, bytes.data(), bytes.size(), 0); - if (size < 0) return stopping_ ? SessionResult::Stopped : SessionResult::Socket; - if (stopping_) return SessionResult::Stopped; - const auto received = std::chrono::steady_clock::now(); - RawRecord record{}; - try { record = decode_record(bytes.data(), static_cast(size)); } - catch (const ProtocolError &) { std::lock_guard lock(health_mutex_); ++health_.malformed_count; if (++consecutive_malformed_ >= kMalformedStormThreshold) return SessionResult::MalformedStorm; continue; } - consecutive_malformed_ = 0; recovered_.store(true, std::memory_order_release); deadline = received + deadline_duration; - if (const auto outcome = handle_record(record, received)) return *outcome; - } - return SessionResult::Stopped; -} -std::optional NetFTClient::handle_record(const RawRecord & record, std::chrono::steady_clock::time_point received) -{ - std::unique_lock record_lock(record_mutex_); - const auto rdt_observation = rdt_->observe(record.rdt_sequence); const auto ft_observation = ft_->observe(record.ft_sequence); - const auto severity = classify_status(record.status); - { std::lock_guard lock(health_mutex_); ++health_.received_count; last_valid_ = received; receive_times_.push_back(received); while (!receive_times_.empty() && received - receive_times_.front() > std::chrono::seconds{1}) receive_times_.pop_front(); health_.receive_rate = static_cast(receive_times_.size()); health_.last_rdt_sequence = record.rdt_sequence; health_.last_ft_sequence = record.ft_sequence; health_.last_status = record.status; - if (rdt_observation.kind == SequenceKind::Gap) health_.lost_count += rdt_observation.gap; - if (rdt_observation.kind == SequenceKind::Duplicate) ++health_.duplicate_count; - if (rdt_observation.kind == SequenceKind::OutOfOrder) ++health_.out_of_order_count; - if (severity == DiagnosticSeverity::Warn) ++health_.warning_count; - if (severity == DiagnosticSeverity::Error) ++health_.device_error_count; - if (ft_observation.kind == FtSequenceKind::Stall) { ++health_.ft_stall_count; health_.last_ft_progress = "stall"; } - else if (ft_observation.kind == FtSequenceKind::Backward) { ++health_.ft_backward_count; health_.last_ft_progress = "backward"; } - else if (ft_observation.kind == FtSequenceKind::Restart) { ++health_.ft_restart_count; health_.last_ft_progress = "restart"; } - else health_.last_ft_progress = ft_observation.kind == FtSequenceKind::First ? "first" : "forward"; - } - std::optional outcome; - if (severity == DiagnosticSeverity::Error) outcome = SessionResult::SeriousStatus; - else if (ft_observation.kind == FtSequenceKind::Stall) outcome = SessionResult::FtStall; - else if (ft_observation.kind == FtSequenceKind::Backward) outcome = SessionResult::FtBackward; - const bool drop_reconnect_ft_discontinuity = - (ft_observation.kind == FtSequenceKind::Stall || ft_observation.kind == FtSequenceKind::Backward) && - config_.recovery_policy == RecoveryPolicy::Reconnect; - if (drop_reconnect_ft_discontinuity && - (outcome == SessionResult::FtStall || outcome == SessionResult::FtBackward)) outcome.reset(); - const bool ordered = rdt_observation.kind != SequenceKind::Duplicate && rdt_observation.kind != SequenceKind::OutOfOrder; - set_state(ClientState::Streaming); - const bool eligible = ordered && !drop_reconnect_ft_discontinuity && (!outcome || (outcome == SessionResult::SeriousStatus && config_.publish_on_error)); - if (!eligible) return outcome; - WrenchSample sample{{record.rdt_sequence}, {record.ft_sequence}, {record.status}, {record.fx / config_.counts_per_force, record.fy / config_.counts_per_force, record.fz / config_.counts_per_force}, {record.tx / config_.counts_per_torque, record.ty / config_.counts_per_torque, record.tz / config_.counts_per_torque}, received}; - bool publish = true; - if (config_.publish_rate > 0 && last_publish_.time_since_epoch().count() != 0 && received - last_publish_ < std::chrono::duration{1.0 / config_.publish_rate}) publish = false; - if (!publish) { std::lock_guard lock(health_mutex_); ++health_.rate_dropped_count; return outcome; } - record_lock.unlock(); - const auto callback_failure_outcome = [&] { - if (outcome || config_.recovery_policy == RecoveryPolicy::Reconnect) return outcome; - return std::optional{SessionResult::Callback}; - }; - try { - SampleCallback callback = callback_; - if (callback) callback(sample); - } - catch (const std::exception & error) { - record_callback_error(error.what()); - return callback_failure_outcome(); - } - catch (...) { - record_callback_error("sample callback threw"); - return callback_failure_outcome(); - } - { std::lock_guard lock(record_mutex_); last_publish_ = received; } - { std::lock_guard lock(health_mutex_); latest_ = sample; ++health_.published_count; publish_times_.push_back(received); while (!publish_times_.empty() && received - publish_times_.front() > std::chrono::seconds{1}) publish_times_.pop_front(); health_.publish_rate = static_cast(publish_times_.size()); published_generation_ = generation_; } - first_sample_cv_.notify_all(); - return outcome; -} -} // namespace netft_driver diff --git a/src/compat/xml_config.cpp b/src/compat/xml_config.cpp new file mode 100644 index 0000000..abb5a67 --- /dev/null +++ b/src/compat/xml_config.cpp @@ -0,0 +1,289 @@ +// SPDX-License-Identifier: Apache-2.0 +// Derived from the vendored netft-cpp snapshot; see ../core/UPSTREAM and ../core/LICENSE. + +#include "detail/xml_config.hpp" + +#include +#include +#include +#include +#include +#include + +#include "netft/discovery.hpp" + +namespace netft::detail { +namespace { + +constexpr std::size_t kMaximumFieldLength = 128; +constexpr std::array kRequiredTags{"prodname", "cfgcpf", "cfgcpt", "scfgfu", + "scfgtu"}; + +bool is_ascii_whitespace(char character) noexcept { + return character == ' ' || character == '\t' || character == '\n' || character == '\r' || + character == '\f' || character == '\v'; +} + +std::string_view trim_ascii_whitespace(std::string_view value) noexcept { + while (!value.empty() && is_ascii_whitespace(value.front())) { + value.remove_prefix(1); + } + while (!value.empty() && is_ascii_whitespace(value.back())) { + value.remove_suffix(1); + } + return value; +} + +int required_tag_index(std::string_view name) noexcept { + for (std::size_t index = 0; index < kRequiredTags.size(); ++index) { + if (name == kRequiredTags[index]) { + return static_cast(index); + } + } + return -1; +} + +std::size_t find_tag_end(std::string_view xml, std::size_t start) { + char quote{}; + for (std::size_t position = start; position < xml.size(); ++position) { + const char character = xml[position]; + if (quote != '\0') { + if (character == quote) { + quote = '\0'; + } + } else if (character == '\'' || character == '"') { + quote = character; + } else if (character == '>') { + return position; + } + } + throw DiscoveryError("sensor configuration contains unterminated markup"); +} + +struct OpenElement { + std::string_view name; + int required_index{-1}; +}; + +using RequiredFields = std::array; + +void append_required_text(const std::vector &elements, RequiredFields &fields, + std::string_view text) { + if (!elements.empty() && elements.back().required_index >= 0) { + fields[static_cast(elements.back().required_index)].append(text); + } +} + +RequiredFields extract_required_fields(std::string_view xml) { + RequiredFields fields; + std::array seen{}; + std::vector elements; + std::size_t position{}; + + while (position < xml.size()) { + const auto markup = xml.find('<', position); + if (markup == std::string_view::npos) { + append_required_text(elements, fields, xml.substr(position)); + position = xml.size(); + break; + } + append_required_text(elements, fields, xml.substr(position, markup - position)); + + if (xml.substr(markup, 4) == "", markup + 4); + if (end == std::string_view::npos) { + throw DiscoveryError("sensor configuration contains an unterminated comment"); + } + position = end + 3; + continue; + } + if (xml.substr(markup, 9) == "", markup + 9); + if (end == std::string_view::npos) { + throw DiscoveryError("sensor configuration contains unterminated CDATA"); + } + append_required_text(elements, fields, xml.substr(markup + 9, end - markup - 9)); + position = end + 3; + continue; + } + if (xml.substr(markup, 2) == "", markup + 2); + if (end == std::string_view::npos) { + throw DiscoveryError("sensor configuration contains unterminated markup"); + } + position = end + 2; + continue; + } + if (xml.substr(markup, 2) == "= 0) { + throw DiscoveryError("sensor configuration fields must contain text only"); + } + if (required_index >= 0) { + const auto index = static_cast(required_index); + if (seen[index]) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' must appear exactly once"); + } + seen[index] = true; + } + if (!self_closing) { + elements.push_back(OpenElement{name, required_index}); + } + position = end + 1; + } + + if (!elements.empty()) { + throw DiscoveryError("sensor configuration contains malformed element nesting"); + } + for (std::size_t index = 0; index < kRequiredTags.size(); ++index) { + if (!seen[index]) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' must appear exactly once"); + } + const auto value = trim_ascii_whitespace(fields[index]); + if (value.empty()) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' is empty"); + } + if (value.size() > kMaximumFieldLength) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' is too long"); + } + fields[index] = std::string{value}; + } + return fields; +} + +bool is_ascii_digit(char character) noexcept { + return character >= '0' && character <= '9'; +} + +bool is_general_decimal(std::string_view value) noexcept { + std::size_t position{}; + if (position < value.size() && value[position] == '-') { + ++position; + } + + bool has_significand_digit = false; + while (position < value.size() && is_ascii_digit(value[position])) { + has_significand_digit = true; + ++position; + } + if (position < value.size() && value[position] == '.') { + ++position; + while (position < value.size() && is_ascii_digit(value[position])) { + has_significand_digit = true; + ++position; + } + } + if (!has_significand_digit) { + return false; + } + + if (position < value.size() && (value[position] == 'e' || value[position] == 'E')) { + ++position; + if (position < value.size() && (value[position] == '+' || value[position] == '-')) { + ++position; + } + const auto exponent_begin = position; + while (position < value.size() && is_ascii_digit(value[position])) { + ++position; + } + if (position == exponent_begin) { + return false; + } + } + return position == value.size(); +} + +double parse_positive_count(std::string_view value, std::string_view tag) { + double result{}; + std::istringstream input{std::string{value}}; + input.imbue(std::locale::classic()); + input >> std::noskipws >> result; + if (!is_general_decimal(value) || input.fail() || + input.rdbuf()->sgetc() != std::char_traits::eof() || !std::isfinite(result) || + result <= 0.0) { + throw DiscoveryError("sensor configuration field '" + std::string{tag} + + "' must be a finite positive number"); + } + return result; +} + +std::string_view normalize_torque_spelling(std::string_view value) noexcept { + if (value == "Nm") { + return "N-m"; + } + if (value == "Nmm") { + return "N-mm"; + } + if (value == "kg-cm") { + return "kgf-cm"; + } + return value; +} + +ForceUnit parse_force_unit(std::string_view value) { + const auto unit = force_unit_from_string(value); + if (!unit.has_value() || *unit == ForceUnit::Unknown) { + throw DiscoveryError("sensor configuration contains an unknown force unit"); + } + return *unit; +} + +TorqueUnit parse_torque_unit(std::string_view value) { + const auto unit = torque_unit_from_string(normalize_torque_spelling(value)); + if (!unit.has_value() || *unit == TorqueUnit::Unknown) { + throw DiscoveryError("sensor configuration contains an unknown torque unit"); + } + return *unit; +} + +} // namespace + +SensorConfiguration parse_sensor_configuration(std::string_view xml) { + const auto fields = extract_required_fields(xml); + SensorConfiguration configuration; + configuration.product_name = fields[0]; + configuration.calibration.counts_per_force_unit = parse_positive_count(fields[1], "cfgcpf"); + configuration.calibration.counts_per_torque_unit = parse_positive_count(fields[2], "cfgcpt"); + configuration.calibration.force_unit = parse_force_unit(fields[3]); + configuration.calibration.torque_unit = parse_torque_unit(fields[4]); + configuration.source = CalibrationSource::Sensor; + configuration.revision = 1; + return configuration; +} + +} // namespace netft::detail diff --git a/src/core/LICENSE b/src/core/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/src/core/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/src/core/UPSTREAM b/src/core/UPSTREAM new file mode 100644 index 0000000..ad56af6 --- /dev/null +++ b/src/core/UPSTREAM @@ -0,0 +1,5 @@ +repository=https://github.com/netft/netft-cpp.git +tag=v0.1.2 +commit=bb7df8d211ecc36ebc1434da3686897d7b3d73cc +license=Apache-2.0 +paths=include/netft,src diff --git a/src/core/include/netft/client.hpp b/src/core/include/netft/client.hpp new file mode 100644 index 0000000..4a24373 --- /dev/null +++ b/src/core/include/netft/client.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "netft/export.hpp" +#include "netft/types.hpp" + +namespace netft { + +class NETFT_API NotConnectedError : public std::runtime_error { +public: + using std::runtime_error::runtime_error; +}; + +class NETFT_API Client { +public: + using SampleCallback = std::function; + + explicit Client(Config config); + ~Client(); + Client(const Client &) = delete; + Client &operator=(const Client &) = delete; + Client(Client &&) = delete; + Client &operator=(Client &&) = delete; + + void start(SampleCallback callback); + void stop() noexcept; + void bias(); + bool wait_for_first_sample(std::chrono::duration timeout); + [[nodiscard]] bool faulted() const noexcept; + [[nodiscard]] FaultCode fault_code() const noexcept; + [[nodiscard]] HealthSnapshot health() const; + [[nodiscard]] std::optional latest_sample() const; + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace netft diff --git a/src/core/include/netft/discovery.hpp b/src/core/include/netft/discovery.hpp new file mode 100644 index 0000000..4357f93 --- /dev/null +++ b/src/core/include/netft/discovery.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include + +#include "netft/export.hpp" +#include "netft/types.hpp" + +namespace netft { + +struct DiscoveryOptions { + std::string sensor_host{"192.168.1.1"}; + int http_port{80}; + std::chrono::duration connect_timeout{0.5}; + std::chrono::duration total_timeout{1.0}; +}; + +class NETFT_API DiscoveryError : public std::runtime_error { +public: + using std::runtime_error::runtime_error; +}; + +NETFT_API SensorConfiguration discover_sensor(const DiscoveryOptions &options); + +} // namespace netft diff --git a/src/core/include/netft/export.hpp b/src/core/include/netft/export.hpp new file mode 100644 index 0000000..e9de400 --- /dev/null +++ b/src/core/include/netft/export.hpp @@ -0,0 +1,15 @@ +#pragma once + +#ifdef _WIN32 +#if defined(NETFT_BUILDING_LIBRARY) +#define NETFT_API __declspec(dllexport) +#elif defined(NETFT_USING_LIBRARY) +#define NETFT_API __declspec(dllimport) +#else +#define NETFT_API +#endif +#define NETFT_LOCAL +#else +#define NETFT_API __attribute__((visibility("default"))) +#define NETFT_LOCAL __attribute__((visibility("hidden"))) +#endif diff --git a/src/core/include/netft/status.hpp b/src/core/include/netft/status.hpp new file mode 100644 index 0000000..f9387dd --- /dev/null +++ b/src/core/include/netft/status.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include +#include + +#include "netft/export.hpp" + +namespace netft { + +enum class StatusSeverity { Ok, Warn, Error }; + +NETFT_API StatusSeverity classify_status(std::uint32_t status) noexcept; +NETFT_API std::string decode_status(std::uint32_t status); + +} // namespace netft diff --git a/src/core/include/netft/types.hpp b/src/core/include/netft/types.hpp new file mode 100644 index 0000000..c401b8e --- /dev/null +++ b/src/core/include/netft/types.hpp @@ -0,0 +1,106 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "netft/export.hpp" + +namespace netft { + +enum class ForceUnit { Unknown, PoundForce, Newton, KiloPoundForce, KiloNewton, KilogramForce }; +enum class TorqueUnit { + Unknown, + PoundForceInch, + PoundForceFoot, + NewtonMeter, + NewtonMillimeter, + KilogramForceCentimeter, + KiloNewtonMeter +}; +enum class CalibrationSource { Sensor, Override }; +enum class RecoveryPolicy { Reconnect, FailStop }; +enum class ClientState { Stopped, Connecting, Streaming, Backoff, Faulted }; +enum class FaultCode { + None, + SensorConfiguration, + Timeout, + Socket, + SeriousStatus, + FtStall, + FtBackward, + MalformedStorm, + Callback +}; + +struct Calibration { + double counts_per_force_unit{}; + double counts_per_torque_unit{}; + ForceUnit force_unit{ForceUnit::Unknown}; + TorqueUnit torque_unit{TorqueUnit::Unknown}; +}; + +struct SensorConfiguration { + std::string product_name; + Calibration calibration; + CalibrationSource source{CalibrationSource::Sensor}; + std::uint64_t revision{1}; +}; + +struct Config { + std::string sensor_host{"192.168.1.1"}; + int rdt_port{49152}; + int http_port{80}; + std::chrono::duration receive_timeout{0.1}; + std::chrono::duration configuration_connect_timeout{0.5}; + std::chrono::duration configuration_timeout{1.0}; + std::chrono::duration reconnect_initial_delay{0.25}; + std::chrono::duration reconnect_max_delay{5.0}; + double sample_rate_limit_hz{0.0}; + bool deliver_samples_with_error_status{false}; + RecoveryPolicy recovery_policy{RecoveryPolicy::Reconnect}; + std::optional calibration_override; +}; + +struct Sample { + std::uint32_t rdt_sequence{}, ft_sequence{}, status{}; + std::array force{}, torque{}; + ForceUnit force_unit{ForceUnit::Unknown}; + TorqueUnit torque_unit{TorqueUnit::Unknown}; + std::uint64_t configuration_revision{}; + std::chrono::steady_clock::time_point received_at; +}; + +struct HealthSnapshot { + ClientState state{ClientState::Stopped}; + FaultCode fault_code{FaultCode::None}; + std::string sensor_host; + int rdt_port{}; + std::optional sensor_configuration; + std::optional last_rdt_sequence, last_ft_sequence; + std::uint32_t last_status{}; + double receive_rate_hz{}, delivery_rate_hz{}; + std::uint64_t received_count{}, delivered_count{}, rate_limited_count{}; + std::uint64_t device_error_count{}, warning_count{}, lost_count{}; + std::uint64_t duplicate_count{}, out_of_order_count{}, malformed_count{}; + std::uint64_t reconnect_count{}, timeout_count{}, callback_error_count{}; + std::uint64_t ft_stall_count{}, ft_backward_count{}, ft_restart_count{}; + std::uint64_t calibration_change_count{}; + std::optional> last_record_age; + std::string last_error; + std::string last_ft_progress{"unknown"}; +}; + +NETFT_API void validate(const Calibration &calibration); +NETFT_API void validate(const Config &config); +NETFT_API std::string_view to_string(ForceUnit unit) noexcept; +NETFT_API std::string_view to_string(TorqueUnit unit) noexcept; +NETFT_API std::optional force_unit_from_string(std::string_view value) noexcept; +NETFT_API std::optional torque_unit_from_string(std::string_view value) noexcept; +NETFT_API std::string_view to_string(ClientState state) noexcept; +NETFT_API std::string_view to_string(FaultCode code) noexcept; + +} // namespace netft diff --git a/src/core/src/client.cpp b/src/core/src/client.cpp new file mode 100644 index 0000000..e253b28 --- /dev/null +++ b/src/core/src/client.cpp @@ -0,0 +1,31 @@ +#include "netft/client.hpp" + +#include + +#include "detail/client_impl.hpp" + +namespace netft { + +Client::Client(Config config) : impl_(std::make_unique(std::move(config))) {} + +Client::~Client() { stop(); } + +void Client::start(SampleCallback callback) { impl_->start(std::move(callback)); } + +void Client::stop() noexcept { impl_->stop(); } + +void Client::bias() { impl_->bias(); } + +bool Client::wait_for_first_sample(const std::chrono::duration timeout) { + return impl_->wait_for_first_sample(timeout); +} + +bool Client::faulted() const noexcept { return impl_->faulted(); } + +FaultCode Client::fault_code() const noexcept { return impl_->fault_code(); } + +HealthSnapshot Client::health() const { return impl_->health(); } + +std::optional Client::latest_sample() const { return impl_->latest_sample(); } + +} // namespace netft diff --git a/src/core/src/detail/client_impl.cpp b/src/core/src/detail/client_impl.cpp new file mode 100644 index 0000000..8157d17 --- /dev/null +++ b/src/core/src/detail/client_impl.cpp @@ -0,0 +1,727 @@ +#include "detail/client_impl.hpp" + +#include +#include +#include +#include +#include + +#include "detail/protocol.hpp" +#include "netft/discovery.hpp" +#include "netft/status.hpp" + +namespace netft { +namespace { + +constexpr std::uint64_t kMalformedStormThreshold = 10; + +} // namespace + +FaultCode Client::Impl::fault_for(const SessionResult result) noexcept { + switch (result) { + case Client::Impl::SessionResult::SensorConfiguration: + return FaultCode::SensorConfiguration; + case Client::Impl::SessionResult::Timeout: + return FaultCode::Timeout; + case Client::Impl::SessionResult::Socket: + return FaultCode::Socket; + case Client::Impl::SessionResult::SeriousStatus: + return FaultCode::SeriousStatus; + case Client::Impl::SessionResult::FtStall: + return FaultCode::FtStall; + case Client::Impl::SessionResult::FtBackward: + return FaultCode::FtBackward; + case Client::Impl::SessionResult::MalformedStorm: + return FaultCode::MalformedStorm; + case Client::Impl::SessionResult::Callback: + return FaultCode::Callback; + case Client::Impl::SessionResult::Stopped: + return FaultCode::None; + } + return FaultCode::Socket; +} + +const char *Client::Impl::message_for(const SessionResult result) noexcept { + switch (result) { + case Client::Impl::SessionResult::SensorConfiguration: + return "sensor configuration discovery failed"; + case Client::Impl::SessionResult::Timeout: + return "no valid RDT record before timeout"; + case Client::Impl::SessionResult::Socket: + return "UDP socket failure"; + case Client::Impl::SessionResult::SeriousStatus: + return "serious device status"; + case Client::Impl::SessionResult::FtStall: + return "FT sequence stalled"; + case Client::Impl::SessionResult::FtBackward: + return "FT sequence moved backward"; + case Client::Impl::SessionResult::MalformedStorm: + return "malformed packet storm"; + case Client::Impl::SessionResult::Callback: + return "sample callback failed"; + case Client::Impl::SessionResult::Stopped: + return ""; + } + return "UDP socket failure"; +} + +namespace { + +bool same_calibration(const Calibration &left, const Calibration &right) { + return left.counts_per_force_unit == right.counts_per_force_unit && + left.counts_per_torque_unit == right.counts_per_torque_unit && + left.force_unit == right.force_unit && left.torque_unit == right.torque_unit; +} + +void prune_rate_window(std::deque ×, + const std::chrono::steady_clock::time_point now) { + while (!times.empty() && now - times.front() > std::chrono::seconds{1}) { + times.pop_front(); + } +} + +std::string ft_progress_name(const detail::FtSequenceKind kind) { + switch (kind) { + case detail::FtSequenceKind::First: + return "first"; + case detail::FtSequenceKind::Forward: + return "forward"; + case detail::FtSequenceKind::Stall: + return "stall"; + case detail::FtSequenceKind::Backward: + return "backward"; + case detail::FtSequenceKind::Restart: + return "restart"; + } + return "unknown"; +} + +} // namespace + +Client::Impl::Impl(Config config) : config_(std::move(config)) { + validate(config_); + health_.sensor_host = config_.sensor_host; + health_.rdt_port = config_.rdt_port; +} + +Client::Impl::~Impl() { stop(); } + +void Client::Impl::start(SampleCallback callback) { + for (;;) { + std::thread reaped_worker; + { + std::unique_lock lifecycle_lock(lifecycle_mutex_); + if (!worker_exited_.load(std::memory_order_acquire) && + active_worker_id_ == std::this_thread::get_id()) { + throw std::logic_error("client is already running"); + } + lifecycle_cv_.wait(lifecycle_lock, [this] { return !joining_; }); + if (worker_.joinable()) { + if (!worker_exited_.load(std::memory_order_acquire)) { + if (!faulted()) { + throw std::logic_error("client is already running"); + } + lifecycle_cv_.wait(lifecycle_lock, [this] { + return worker_exited_.load(std::memory_order_acquire) || !faulted(); + }); + continue; + } + joining_ = true; + reaped_worker = std::move(worker_); + } else { + const bool previous_stopping = stopping_.load(); + const bool previous_worker_exited = worker_exited_.load(); + const auto previous_fault = fault_code(); + auto previous_callback = std::move(callback_); + std::chrono::steady_clock::time_point previous_last_delivery; + { + std::scoped_lock record_lock(record_mutex_); + previous_last_delivery = last_delivery_at_; + last_delivery_at_ = {}; + } + + HealthSnapshot previous_health; + std::optional previous_latest; + std::chrono::steady_clock::time_point previous_last_record; + std::uint64_t previous_generation{}; + std::uint64_t previous_delivered_generation{}; + std::deque previous_receive_times; + std::deque previous_delivery_times; + std::uint64_t previous_consecutive_malformed{}; + { + std::scoped_lock data_lock(data_mutex_); + previous_health = health_; + previous_latest = latest_; + previous_last_record = last_record_at_; + previous_generation = generation_; + previous_delivered_generation = delivered_generation_; + previous_receive_times = receive_times_; + previous_delivery_times = delivery_times_; + previous_consecutive_malformed = consecutive_malformed_; + + fault_latch_.reset(); + health_.state = ClientState::Connecting; + health_.fault_code = FaultCode::None; + health_.last_error.clear(); + health_.receive_rate_hz = 0.0; + health_.delivery_rate_hz = 0.0; + latest_.reset(); + last_record_at_ = {}; + ++generation_; + delivered_generation_ = 0; + receive_times_.clear(); + delivery_times_.clear(); + consecutive_malformed_ = 0; + } + stopping_ = false; + worker_exited_ = false; + callback_ = std::move(callback); + try { + worker_ = create_worker_thread(); + } catch (...) { + stopping_ = previous_stopping; + worker_exited_ = previous_worker_exited; + callback_ = std::move(previous_callback); + { + std::scoped_lock record_lock(record_mutex_); + last_delivery_at_ = previous_last_delivery; + } + { + std::scoped_lock data_lock(data_mutex_); + health_ = previous_health; + latest_ = previous_latest; + last_record_at_ = previous_last_record; + generation_ = previous_generation; + delivered_generation_ = previous_delivered_generation; + receive_times_ = std::move(previous_receive_times); + delivery_times_ = std::move(previous_delivery_times); + consecutive_malformed_ = previous_consecutive_malformed; + } + fault_latch_.reset(); + if (previous_fault != FaultCode::None) { + static_cast(fault_latch_.publish(previous_fault, previous_health.last_error, + data_mutex_, health_)); + } + first_sample_cv_.notify_all(); + lifecycle_cv_.notify_all(); + throw; + } + active_worker_id_ = worker_.get_id(); + first_sample_cv_.notify_all(); + lifecycle_cv_.notify_all(); + return; + } + } + + reaped_worker.join(); + { + std::scoped_lock lifecycle_lock(lifecycle_mutex_); + active_worker_id_ = {}; + joining_ = false; + } + lifecycle_cv_.notify_all(); + } +} + +std::thread Client::Impl::create_worker_thread() { + if (thread_factory_ != nullptr) { + return thread_factory_(this); + } + return std::thread([this] { run(); }); +} + +void Client::Impl::stop() noexcept { + for (;;) { + std::thread joining_worker; + { + std::unique_lock lifecycle_lock(lifecycle_mutex_); + stopping_ = true; + first_sample_cv_.notify_all(); + lifecycle_cv_.notify_all(); + { + std::scoped_lock command_lock(command_mutex_); + if (session_started_) { + try { + transport_.send(detail::encode_request(detail::Command::StopStreaming)); + } catch (...) { + static_cast(0); + } + session_started_ = false; + } + transport_.shutdown(); + } + + if (!worker_exited_.load(std::memory_order_acquire) && + active_worker_id_ == std::this_thread::get_id()) { + return; + } + if (joining_) { + lifecycle_cv_.wait(lifecycle_lock, [this] { return !joining_; }); + continue; + } + if (!worker_.joinable()) { + if (!faulted()) { + std::scoped_lock data_lock(data_mutex_); + health_.state = ClientState::Stopped; + } + return; + } + joining_ = true; + joining_worker = std::move(worker_); + } + + joining_worker.join(); + { + std::scoped_lock lifecycle_lock(lifecycle_mutex_); + if (!faulted()) { + std::scoped_lock data_lock(data_mutex_); + health_.state = ClientState::Stopped; + } + active_worker_id_ = {}; + joining_ = false; + } + lifecycle_cv_.notify_all(); + first_sample_cv_.notify_all(); + return; + } +} + +void Client::Impl::bias() { + std::scoped_lock command_lock(command_mutex_); + std::scoped_lock record_lock(record_mutex_); + { + std::scoped_lock data_lock(data_mutex_); + if (stopping_ || !session_started_ || faulted() || health_.state != ClientState::Streaming) { + throw NotConnectedError("client is not streaming"); + } + } + transport_.send(detail::encode_request(detail::Command::SetSoftwareBias)); + transport_.send(detail::encode_request(detail::Command::StartRealtime)); + rdt_sequence_.reset(); +} + +bool Client::Impl::wait_for_first_sample(const std::chrono::duration timeout) { + std::unique_lock data_lock(data_mutex_); + const auto captured_generation = generation_; + if (captured_generation == 0) { + return false; + } + first_sample_cv_.wait_for(data_lock, timeout, [this, captured_generation] { + return generation_ != captured_generation || delivered_generation_ == captured_generation || + stopping_ || faulted(); + }); + if (const auto hook = wait_wake_test_hook_) { + data_lock.unlock(); + hook(this, captured_generation); + data_lock.lock(); + } + if (generation_ != captured_generation) { + return false; + } + return delivered_generation_ == captured_generation; +} + +bool Client::Impl::faulted() const noexcept { return fault_latch_.faulted(); } + +FaultCode Client::Impl::fault_code() const noexcept { return fault_latch_.code(); } + +HealthSnapshot Client::Impl::health() const { + std::scoped_lock data_lock(data_mutex_); + const auto now = std::chrono::steady_clock::now(); + prune_rate_window(receive_times_, now); + prune_rate_window(delivery_times_, now); + auto snapshot = health_; + snapshot.fault_code = fault_code(); + if (snapshot.fault_code != FaultCode::None) { + snapshot.state = ClientState::Faulted; + } + snapshot.receive_rate_hz = static_cast(receive_times_.size()); + snapshot.delivery_rate_hz = static_cast(delivery_times_.size()); + if (last_record_at_ != std::chrono::steady_clock::time_point{}) { + snapshot.last_record_age = now - last_record_at_; + } + return snapshot; +} + +std::optional Client::Impl::latest_sample() const { + std::scoped_lock data_lock(data_mutex_); + return latest_; +} + +SensorConfiguration Client::Impl::configuration_for_session() { + if (config_.calibration_override) { + return SensorConfiguration{"", *config_.calibration_override, CalibrationSource::Override, 1}; + } + DiscoveryOptions options; + options.sensor_host = config_.sensor_host; + options.http_port = config_.http_port; + options.connect_timeout = config_.configuration_connect_timeout; + options.total_timeout = config_.configuration_timeout; + return discover_sensor(options); +} + +void Client::Impl::apply_configuration(SensorConfiguration configuration) { + std::scoped_lock data_lock(data_mutex_); + if (health_.sensor_configuration) { + const auto &previous = *health_.sensor_configuration; + if (same_calibration(previous.calibration, configuration.calibration)) { + configuration.revision = previous.revision; + } else { + configuration.revision = previous.revision + 1; + ++health_.calibration_change_count; + } + } else { + configuration.revision = 1; + } + health_.sensor_configuration = std::move(configuration); +} + +void Client::Impl::run() noexcept { + auto backoff = config_.reconnect_initial_delay; + try { + while (!stopping_ && !faulted()) { + auto outcome = receive_session(); + close_session(); + + if (outcome.message.empty()) { + outcome.message = message_for(outcome.result); + } + if (outcome.result != SessionResult::Stopped && + config_.recovery_policy == RecoveryPolicy::FailStop) { + set_fault(fault_for(outcome.result), std::move(outcome.message)); + break; + } + if (outcome.result == SessionResult::Stopped || stopping_) { + break; + } + + if (outcome.received_valid_record) { + backoff = config_.reconnect_initial_delay; + } + { + std::scoped_lock data_lock(data_mutex_); + health_.last_error = std::move(outcome.message); + health_.state = ClientState::Backoff; + ++health_.reconnect_count; + } + + std::unique_lock data_lock(data_mutex_); + first_sample_cv_.wait_for(data_lock, backoff, [this] { return stopping_.load(); }); + data_lock.unlock(); + backoff = std::min(backoff * 2.0, config_.reconnect_max_delay); + } + } catch (const std::exception &error) { + if (!stopping_) { + set_fault(FaultCode::Socket, error.what()); + } + } catch (...) { + if (!stopping_) { + set_fault(FaultCode::Socket, "unknown client failure"); + } + } + finish_session(); +} + +Client::Impl::SessionOutcome Client::Impl::receive_session() { + bool received_valid_record = false; + { + std::scoped_lock data_lock(data_mutex_); + health_.state = ClientState::Connecting; + } + if (stopping_) { + return {SessionResult::Stopped, {}, false}; + } + + try { + apply_configuration(configuration_for_session()); + } catch (const DiscoveryError &error) { + return {stopping_ ? SessionResult::Stopped : SessionResult::SensorConfiguration, error.what(), + false}; + } + if (stopping_) { + return {SessionResult::Stopped, {}, false}; + } + + try { + transport_.connect(config_.sensor_host, config_.rdt_port); + std::scoped_lock command_lock(command_mutex_); + if (stopping_) { + return {SessionResult::Stopped, {}, false}; + } + transport_.send(detail::encode_request(detail::Command::StartRealtime)); + session_started_ = true; + } catch (const std::exception &error) { + return {stopping_ ? SessionResult::Stopped : SessionResult::Socket, error.what(), false}; + } + + { + std::scoped_lock record_lock(record_mutex_); + rdt_sequence_.reset(); + ft_sequence_.begin_session(); + } + + const auto timeout = config_.receive_timeout; + auto deadline = std::chrono::steady_clock::now() + timeout; + std::array buffer{}; + while (!stopping_) { + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) { + std::scoped_lock data_lock(data_mutex_); + if (stopping_) { + return {SessionResult::Stopped, {}, received_valid_record}; + } + ++health_.timeout_count; + return {SessionResult::Timeout, {}, received_valid_record}; + } + + std::size_t size{}; + try { + size = transport_.receive(buffer.data(), buffer.size(), deadline - now); + } catch (const std::exception &error) { + return {stopping_ ? SessionResult::Stopped : SessionResult::Socket, error.what(), + received_valid_record}; + } + if (stopping_) { + return {SessionResult::Stopped, {}, received_valid_record}; + } + if (size == 0) { + std::scoped_lock data_lock(data_mutex_); + if (stopping_) { + return {SessionResult::Stopped, {}, received_valid_record}; + } + ++health_.timeout_count; + return {SessionResult::Timeout, {}, received_valid_record}; + } + + detail::RawRecord record; + try { + record = detail::decode_record(buffer.data(), size); + } catch (const detail::ProtocolError &) { + std::scoped_lock data_lock(data_mutex_); + ++health_.malformed_count; + if (++consecutive_malformed_ >= kMalformedStormThreshold) { + return {SessionResult::MalformedStorm, {}, received_valid_record}; + } + continue; + } + + const auto received_at = std::chrono::steady_clock::now(); + { + std::scoped_lock data_lock(data_mutex_); + consecutive_malformed_ = 0; + } + received_valid_record = true; + deadline = received_at + timeout; + if (const auto outcome = handle_record(record, received_at)) { + return {*outcome, {}, received_valid_record}; + } + } + return {SessionResult::Stopped, {}, received_valid_record}; +} + +std::optional +Client::Impl::handle_record(const detail::RawRecord &record, + const std::chrono::steady_clock::time_point received_at) { + SensorConfiguration configuration; + bool deliver = true; + std::optional outcome; + { + std::unique_lock record_lock(record_mutex_); + const auto rdt = rdt_sequence_.observe(record.rdt_sequence); + const auto ft = ft_sequence_.observe(record.ft_sequence); + const auto severity = classify_status(record.status); + + { + std::scoped_lock data_lock(data_mutex_); + if (!health_.sensor_configuration) { + throw std::logic_error("sensor configuration is unavailable while streaming"); + } + configuration = *health_.sensor_configuration; + health_.state = ClientState::Streaming; + ++health_.received_count; + receive_times_.push_back(received_at); + prune_rate_window(receive_times_, received_at); + health_.receive_rate_hz = static_cast(receive_times_.size()); + health_.last_rdt_sequence = record.rdt_sequence; + health_.last_ft_sequence = record.ft_sequence; + health_.last_status = record.status; + last_record_at_ = received_at; + + switch (rdt.kind) { + case detail::SequenceKind::Gap: + health_.lost_count += rdt.gap; + break; + case detail::SequenceKind::Duplicate: + ++health_.duplicate_count; + deliver = false; + break; + case detail::SequenceKind::OutOfOrder: + ++health_.out_of_order_count; + deliver = false; + break; + case detail::SequenceKind::First: + case detail::SequenceKind::Contiguous: + break; + } + + health_.last_ft_progress = ft_progress_name(ft.kind); + switch (ft.kind) { + case detail::FtSequenceKind::Stall: + ++health_.ft_stall_count; + break; + case detail::FtSequenceKind::Backward: + ++health_.ft_backward_count; + break; + case detail::FtSequenceKind::Restart: + ++health_.ft_restart_count; + break; + case detail::FtSequenceKind::First: + case detail::FtSequenceKind::Forward: + break; + } + + if (severity == StatusSeverity::Warn) { + ++health_.warning_count; + } else if (severity == StatusSeverity::Error) { + ++health_.device_error_count; + deliver = config_.deliver_samples_with_error_status && deliver; + outcome = SessionResult::SeriousStatus; + } + + if (!outcome && ft.kind == detail::FtSequenceKind::Stall) { + outcome = SessionResult::FtStall; + } else if (!outcome && ft.kind == detail::FtSequenceKind::Backward) { + outcome = SessionResult::FtBackward; + } + + const bool drop_reconnect_ft_discontinuity = + config_.recovery_policy == RecoveryPolicy::Reconnect && + (ft.kind == detail::FtSequenceKind::Stall || ft.kind == detail::FtSequenceKind::Backward); + if (drop_reconnect_ft_discontinuity) { + deliver = false; + if (outcome == SessionResult::FtStall || outcome == SessionResult::FtBackward) { + outcome.reset(); + } + } + + if (deliver && config_.sample_rate_limit_hz > 0.0 && + last_delivery_at_ != std::chrono::steady_clock::time_point{} && + received_at - last_delivery_at_ < + std::chrono::duration{1.0 / config_.sample_rate_limit_hz}) { + deliver = false; + ++health_.rate_limited_count; + } + } + record_lock.unlock(); + } + + if (!deliver) { + return outcome; + } + + const auto &calibration = configuration.calibration; + Sample sample; + sample.rdt_sequence = record.rdt_sequence; + sample.ft_sequence = record.ft_sequence; + sample.status = record.status; + sample.force = {record.fx / calibration.counts_per_force_unit, + record.fy / calibration.counts_per_force_unit, + record.fz / calibration.counts_per_force_unit}; + sample.torque = {record.tx / calibration.counts_per_torque_unit, + record.ty / calibration.counts_per_torque_unit, + record.tz / calibration.counts_per_torque_unit}; + sample.force_unit = calibration.force_unit; + sample.torque_unit = calibration.torque_unit; + sample.configuration_revision = configuration.revision; + sample.received_at = received_at; + + const auto callback_failure_outcome = [&] { + if (outcome || config_.recovery_policy == RecoveryPolicy::Reconnect) { + return outcome; + } + return std::optional{SessionResult::Callback}; + }; + + try { + SampleCallback callback = callback_; + if (callback) { + callback(sample); + } + } catch (const std::exception &error) { + record_callback_error(error.what()); + return callback_failure_outcome(); + } catch (...) { + record_callback_error("sample callback failed"); + return callback_failure_outcome(); + } + + { + std::scoped_lock record_lock(record_mutex_); + last_delivery_at_ = received_at; + std::scoped_lock data_lock(data_mutex_); + latest_ = sample; + ++health_.delivered_count; + delivery_times_.push_back(received_at); + prune_rate_window(delivery_times_, received_at); + health_.delivery_rate_hz = static_cast(delivery_times_.size()); + delivered_generation_ = generation_; + } + first_sample_cv_.notify_all(); + lifecycle_cv_.notify_all(); + return outcome; +} + +void Client::Impl::set_fault(FaultCode code, std::string message) noexcept { + const bool published = fault_latch_.publish(code, std::move(message), data_mutex_, health_); + first_sample_cv_.notify_all(); + lifecycle_cv_.notify_all(); + if (published) { + if (const auto hook = fault_published_test_hook_) { + hook(this); + } + } +} + +void Client::Impl::record_callback_error(const char *message) noexcept { + try { + std::scoped_lock data_lock(data_mutex_); + ++health_.callback_error_count; + try { + health_.last_error = message; + } catch (...) { + static_cast(0); + } + } catch (...) { + static_cast(0); + } +} + +void Client::Impl::close_session() noexcept { + std::scoped_lock command_lock(command_mutex_); + if (session_started_) { + try { + transport_.send(detail::encode_request(detail::Command::StopStreaming)); + } catch (...) { + static_cast(0); + } + session_started_ = false; + } + transport_.close(); +} + +void Client::Impl::finish_session() noexcept { + close_session(); + worker_exited_.store(true, std::memory_order_release); + if (!faulted()) { + try { + std::scoped_lock data_lock(data_mutex_); + health_.state = ClientState::Stopped; + } catch (...) { + static_cast(0); + } + } + first_sample_cv_.notify_all(); + lifecycle_cv_.notify_all(); +} + +} // namespace netft diff --git a/src/core/src/detail/client_impl.hpp b/src/core/src/detail/client_impl.hpp new file mode 100644 index 0000000..9eb871d --- /dev/null +++ b/src/core/src/detail/client_impl.hpp @@ -0,0 +1,107 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "detail/fault_latch.hpp" +#include "detail/posix_transport.hpp" +#include "detail/protocol.hpp" +#include "detail/sequence.hpp" +#include "netft/client.hpp" + +namespace netft { + +class NETFT_LOCAL Client::Impl { +public: + explicit Impl(Config config); + ~Impl(); + + void start(SampleCallback callback); + void stop() noexcept; + void bias(); + bool wait_for_first_sample(std::chrono::duration timeout); + bool faulted() const noexcept; + FaultCode fault_code() const noexcept; + HealthSnapshot health() const; + std::optional latest_sample() const; + +private: + enum class SessionResult { + Stopped, + SensorConfiguration, + Timeout, + Socket, + SeriousStatus, + FtStall, + FtBackward, + MalformedStorm, + Callback, + }; + + struct SessionOutcome { + SessionResult result{SessionResult::Stopped}; + std::string message; + bool received_valid_record{false}; + }; + + void run() noexcept; + std::thread create_worker_thread(); + static FaultCode fault_for(SessionResult result) noexcept; + static const char *message_for(SessionResult result) noexcept; + SessionOutcome receive_session(); + SensorConfiguration configuration_for_session(); + void apply_configuration(SensorConfiguration configuration); + std::optional handle_record(const detail::RawRecord &record, + std::chrono::steady_clock::time_point received_at); + void close_session() noexcept; + void set_fault(FaultCode code, std::string message) noexcept; + void record_callback_error(const char *message) noexcept; + void finish_session() noexcept; + + using ThreadFactory = std::thread (*)(Impl *); + using FaultPublishedHook = void (*)(Impl *) noexcept; + using WaitWakeHook = void (*)(Impl *, std::uint64_t) noexcept; + + Config config_; + mutable std::mutex lifecycle_mutex_; + std::condition_variable lifecycle_cv_; + // Locks are acquired in command -> record -> data order. Callbacks run after + // all three have been released. + mutable std::mutex command_mutex_; + mutable std::mutex record_mutex_; + mutable std::mutex data_mutex_; + std::condition_variable first_sample_cv_; + std::thread worker_; + ThreadFactory thread_factory_{nullptr}; + // Internal synchronization hooks used only by lifecycle tests. Client's + // installed API remains opaque and does not expose these seams. + FaultPublishedHook fault_published_test_hook_{nullptr}; + WaitWakeHook wait_wake_test_hook_{nullptr}; + std::thread::id active_worker_id_; + bool joining_{false}; + SampleCallback callback_; + detail::PosixTransport transport_; + detail::RdtSequenceTracker rdt_sequence_; + detail::FtSequenceTracker ft_sequence_; + std::atomic stopping_{false}; + std::atomic worker_exited_{true}; + bool session_started_{false}; + detail::FaultLatch fault_latch_; + HealthSnapshot health_; + std::optional latest_; + std::chrono::steady_clock::time_point last_delivery_at_; + std::chrono::steady_clock::time_point last_record_at_; + std::uint64_t generation_{0}; + std::uint64_t delivered_generation_{0}; + mutable std::deque receive_times_; + mutable std::deque delivery_times_; + std::uint64_t consecutive_malformed_{}; +}; + +} // namespace netft diff --git a/src/core/src/detail/fault_latch.cpp b/src/core/src/detail/fault_latch.cpp new file mode 100644 index 0000000..d7abdbc --- /dev/null +++ b/src/core/src/detail/fault_latch.cpp @@ -0,0 +1,33 @@ +#include "detail/fault_latch.hpp" + +#include + +namespace netft::detail { + +bool FaultLatch::publish(const FaultCode code, std::string message, std::mutex &data_mutex, + HealthSnapshot &health) noexcept { + try { + std::scoped_lock data_lock(data_mutex); + FaultCode expected = FaultCode::None; + if (!code_.compare_exchange_strong(expected, code, std::memory_order_acq_rel, + std::memory_order_acquire)) { + return false; + } + health.state = ClientState::Faulted; + health.fault_code = code; + health.last_error.swap(message); + return true; + } catch (...) { + return false; + } +} + +void FaultLatch::reset() noexcept { code_.store(FaultCode::None, std::memory_order_release); } + +bool FaultLatch::faulted() const noexcept { + return code_.load(std::memory_order_acquire) != FaultCode::None; +} + +FaultCode FaultLatch::code() const noexcept { return code_.load(std::memory_order_acquire); } + +} // namespace netft::detail diff --git a/src/core/src/detail/fault_latch.hpp b/src/core/src/detail/fault_latch.hpp new file mode 100644 index 0000000..0916466 --- /dev/null +++ b/src/core/src/detail/fault_latch.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include + +#include "netft/types.hpp" + +namespace netft::detail { + +class FaultLatch { +public: + FaultLatch() = default; + FaultLatch(const FaultLatch &) = delete; + FaultLatch &operator=(const FaultLatch &) = delete; + + bool publish(FaultCode code, std::string message, std::mutex &data_mutex, + HealthSnapshot &health) noexcept; + void reset() noexcept; + [[nodiscard]] bool faulted() const noexcept; + [[nodiscard]] FaultCode code() const noexcept; + +private: + std::atomic code_{FaultCode::None}; +}; + +} // namespace netft::detail diff --git a/src/core/src/detail/posix_transport.cpp b/src/core/src/detail/posix_transport.cpp new file mode 100644 index 0000000..26620f7 --- /dev/null +++ b/src/core/src/detail/posix_transport.cpp @@ -0,0 +1,138 @@ +#include "detail/posix_transport.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace netft::detail { +namespace { + +std::runtime_error socket_error(const char *operation) { + return std::runtime_error(std::string{operation} + ": " + std::strerror(errno)); +} + +int timeout_milliseconds(const std::chrono::duration timeout) { + const auto milliseconds = std::ceil(timeout.count() * 1000.0); + if (milliseconds >= static_cast(std::numeric_limits::max())) { + return std::numeric_limits::max(); + } + return static_cast(milliseconds); +} + +} // namespace + +PosixTransport::~PosixTransport() { close(); } + +void PosixTransport::connect(const std::string &host, const int port) { + addrinfo hints{}; + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + addrinfo *addresses = nullptr; + const auto service = std::to_string(port); + const int result = ::getaddrinfo(host.c_str(), service.c_str(), &hints, &addresses); + if (result != 0) { + throw std::runtime_error(std::string{"failed to resolve sensor: "} + ::gai_strerror(result)); + } + + int connected_socket = -1; + int last_error = 0; + for (auto *address = addresses; address != nullptr; address = address->ai_next) { + connected_socket = ::socket(address->ai_family, address->ai_socktype, address->ai_protocol); + if (connected_socket < 0) { + last_error = errno; + continue; + } + if (::connect(connected_socket, address->ai_addr, address->ai_addrlen) == 0) { + break; + } + last_error = errno; + ::close(connected_socket); + connected_socket = -1; + } + ::freeaddrinfo(addresses); + + if (connected_socket < 0) { + errno = last_error; + throw socket_error("failed to connect UDP socket"); + } + + std::scoped_lock lock(mutex_); + if (socket_ >= 0) { + ::close(socket_); + } + socket_ = connected_socket; +} + +void PosixTransport::send(const std::array &request) { + std::scoped_lock lock(mutex_); + if (socket_ < 0) { + throw std::runtime_error("UDP socket is not connected"); + } + const auto sent = ::send(socket_, request.data(), request.size(), 0); + if (sent < 0) { + throw socket_error("failed to send UDP request"); + } + if (static_cast(sent) != request.size()) { + throw std::runtime_error("short UDP request write"); + } +} + +std::size_t PosixTransport::receive(std::uint8_t *data, const std::size_t capacity, + const std::chrono::duration timeout) { + int socket = -1; + { + std::scoped_lock lock(mutex_); + socket = socket_; + } + if (socket < 0) { + throw std::runtime_error("UDP socket is not connected"); + } + + pollfd descriptor{socket, POLLIN, 0}; + int poll_result{}; + do { + poll_result = ::poll(&descriptor, 1, timeout_milliseconds(timeout)); + } while (poll_result < 0 && errno == EINTR); + if (poll_result < 0) { + throw socket_error("failed to wait for UDP record"); + } + if (poll_result == 0) { + return 0; + } + if ((descriptor.revents & POLLNVAL) != 0) { + throw std::runtime_error("UDP socket became invalid"); + } + + const auto received = ::recv(socket, data, capacity, 0); + if (received < 0) { + if (errno == EINTR) { + return 0; + } + throw socket_error("failed to receive UDP record"); + } + return static_cast(received); +} + +void PosixTransport::shutdown() noexcept { + std::scoped_lock lock(mutex_); + if (socket_ >= 0) { + static_cast(::shutdown(socket_, SHUT_RDWR)); + } +} + +void PosixTransport::close() noexcept { + std::scoped_lock lock(mutex_); + if (socket_ >= 0) { + ::close(socket_); + socket_ = -1; + } +} + +} // namespace netft::detail diff --git a/src/core/src/detail/posix_transport.hpp b/src/core/src/detail/posix_transport.hpp new file mode 100644 index 0000000..68b59ee --- /dev/null +++ b/src/core/src/detail/posix_transport.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace netft::detail { + +class PosixTransport { +public: + PosixTransport() = default; + ~PosixTransport(); + PosixTransport(const PosixTransport &) = delete; + PosixTransport &operator=(const PosixTransport &) = delete; + + void connect(const std::string &host, int port); + void send(const std::array &request); + std::size_t receive(std::uint8_t *data, std::size_t capacity, + std::chrono::duration timeout); + void shutdown() noexcept; + void close() noexcept; + +private: + mutable std::mutex mutex_; + int socket_{-1}; +}; + +} // namespace netft::detail diff --git a/src/core/src/detail/protocol.cpp b/src/core/src/detail/protocol.cpp new file mode 100644 index 0000000..301fc5c --- /dev/null +++ b/src/core/src/detail/protocol.cpp @@ -0,0 +1,43 @@ +#include "detail/protocol.hpp" + +#include + +namespace netft::detail { +namespace { + +std::uint32_t read_u32(const std::uint8_t *data) { + return (static_cast(data[0]) << 24U) | + (static_cast(data[1]) << 16U) | + (static_cast(data[2]) << 8U) | static_cast(data[3]); +} + +std::int32_t read_i32(const std::uint8_t *data) { + const std::uint32_t value = read_u32(data); + if (value <= 0x7fffffffU) { + return static_cast(value); + } + return -static_cast(~value) - 1; +} + +} // namespace + +std::array encode_request(const Command command, const std::uint32_t count) { + const auto value = static_cast(command); + return {{0x12, 0x34, static_cast(value >> 8U), static_cast(value), + static_cast(count >> 24U), static_cast(count >> 16U), + static_cast(count >> 8U), static_cast(count)}}; +} + +RawRecord decode_record(const std::uint8_t *data, const std::size_t size) { + if (size != 36) { + throw ProtocolError{"RDT record must be exactly 36 bytes, received " + std::to_string(size)}; + } + if (data == nullptr) { + throw ProtocolError{"RDT record data must not be null"}; + } + return {read_u32(data), read_u32(data + 4), read_u32(data + 8), + read_i32(data + 12), read_i32(data + 16), read_i32(data + 20), + read_i32(data + 24), read_i32(data + 28), read_i32(data + 32)}; +} + +} // namespace netft::detail diff --git a/include/netft_driver/protocol.hpp b/src/core/src/detail/protocol.hpp similarity index 82% rename from include/netft_driver/protocol.hpp rename to src/core/src/detail/protocol.hpp index 175d21f..ceeab36 100644 --- a/include/netft_driver/protocol.hpp +++ b/src/core/src/detail/protocol.hpp @@ -5,7 +5,7 @@ #include #include -namespace netft_driver { +namespace netft::detail { enum class Command : std::uint16_t { StopStreaming = 0x0000, @@ -26,6 +26,6 @@ class ProtocolError : public std::runtime_error { }; std::array encode_request(Command command, std::uint32_t count = 0); -RawRecord decode_record(const std::uint8_t * data, std::size_t size); +RawRecord decode_record(const std::uint8_t *data, std::size_t size); -} // namespace netft_driver +} // namespace netft::detail diff --git a/src/core/src/detail/sequence.cpp b/src/core/src/detail/sequence.cpp new file mode 100644 index 0000000..02c07a8 --- /dev/null +++ b/src/core/src/detail/sequence.cpp @@ -0,0 +1,67 @@ +#include "detail/sequence.hpp" + +namespace netft::detail { +namespace { + +constexpr std::uint32_t kRestartLowMax = 0x0000ffffU; +constexpr std::uint32_t kRestartMinBaseline = kRestartLowMax + 1U; + +} // namespace + +SequenceObservation RdtSequenceTracker::observe(const std::uint32_t current) { + if (!has_last_) { + has_last_ = true; + previous_ = current; + return {SequenceKind::First, 0}; + } + + const std::uint32_t delta = current - previous_; + if (delta == 0) { + return {SequenceKind::Duplicate, 0}; + } + if (delta < 0x80000000U) { + previous_ = current; + return delta == 1 ? SequenceObservation{SequenceKind::Contiguous, 0} + : SequenceObservation{SequenceKind::Gap, delta - 1}; + } + return {SequenceKind::OutOfOrder, 0}; +} + +void RdtSequenceTracker::reset() { has_last_ = false; } + +void FtSequenceTracker::begin_session() { has_restart_candidate_ = false; } + +FtSequenceObservation FtSequenceTracker::observe(const std::uint32_t current) { + if (!has_last_) { + has_last_ = true; + previous_ = current; + return {FtSequenceKind::First}; + } + if (current == previous_) { + has_restart_candidate_ = false; + return {FtSequenceKind::Stall}; + } + + const std::uint32_t delta = current - previous_; + if (delta < 0x80000000U) { + previous_ = current; + has_restart_candidate_ = false; + return {FtSequenceKind::Forward}; + } + + if (previous_ >= kRestartMinBaseline && has_restart_candidate_ && + restart_candidate_ <= kRestartLowMax && current <= kRestartLowMax) { + const std::uint32_t candidate_delta = current - restart_candidate_; + if (candidate_delta > 0 && candidate_delta < 0x80000000U) { + previous_ = current; + has_restart_candidate_ = false; + return {FtSequenceKind::Restart}; + } + } + + restart_candidate_ = current; + has_restart_candidate_ = true; + return {FtSequenceKind::Backward}; +} + +} // namespace netft::detail diff --git a/src/core/src/detail/sequence.hpp b/src/core/src/detail/sequence.hpp new file mode 100644 index 0000000..726d449 --- /dev/null +++ b/src/core/src/detail/sequence.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include + +namespace netft::detail { + +enum class SequenceKind { First, Contiguous, Gap, Duplicate, OutOfOrder }; + +struct SequenceObservation { + SequenceKind kind; + std::uint32_t gap{0}; +}; + +class RdtSequenceTracker { +public: + SequenceObservation observe(std::uint32_t current); + void reset(); + [[nodiscard]] bool has_last() const { return has_last_; } + [[nodiscard]] std::uint32_t last() const { return previous_; } + +private: + bool has_last_{false}; + std::uint32_t previous_{}; +}; + +enum class FtSequenceKind { First, Forward, Stall, Backward, Restart }; + +struct FtSequenceObservation { + FtSequenceKind kind; +}; + +class FtSequenceTracker { +public: + FtSequenceObservation observe(std::uint32_t current); + void begin_session(); + [[nodiscard]] bool has_last() const { return has_last_; } + [[nodiscard]] std::uint32_t last() const { return previous_; } + +private: + bool has_last_{false}; + std::uint32_t previous_{}; + bool has_restart_candidate_{false}; + std::uint32_t restart_candidate_{}; +}; + +} // namespace netft::detail diff --git a/src/core/src/detail/xml_config.cpp b/src/core/src/detail/xml_config.cpp new file mode 100644 index 0000000..124c241 --- /dev/null +++ b/src/core/src/detail/xml_config.cpp @@ -0,0 +1,242 @@ +#include "detail/xml_config.hpp" + +#include +#include +#include +#include +#include +#include + +#include "netft/discovery.hpp" + +namespace netft::detail { +namespace { + +constexpr std::size_t kMaximumFieldLength = 128; +constexpr std::array kRequiredTags{"prodname", "cfgcpf", "cfgcpt", "scfgfu", + "scfgtu"}; + +bool is_ascii_whitespace(char character) noexcept { + return character == ' ' || character == '\t' || character == '\n' || character == '\r' || + character == '\f' || character == '\v'; +} + +std::string_view trim_ascii_whitespace(std::string_view value) noexcept { + while (!value.empty() && is_ascii_whitespace(value.front())) { + value.remove_prefix(1); + } + while (!value.empty() && is_ascii_whitespace(value.back())) { + value.remove_suffix(1); + } + return value; +} + +int required_tag_index(std::string_view name) noexcept { + for (std::size_t index = 0; index < kRequiredTags.size(); ++index) { + if (name == kRequiredTags[index]) { + return static_cast(index); + } + } + return -1; +} + +std::size_t find_tag_end(std::string_view xml, std::size_t start) { + char quote{}; + for (std::size_t position = start; position < xml.size(); ++position) { + const char character = xml[position]; + if (quote != '\0') { + if (character == quote) { + quote = '\0'; + } + } else if (character == '\'' || character == '"') { + quote = character; + } else if (character == '>') { + return position; + } + } + throw DiscoveryError("sensor configuration contains unterminated markup"); +} + +struct OpenElement { + std::string_view name; + int required_index{-1}; +}; + +using RequiredFields = std::array; + +void append_required_text(const std::vector &elements, RequiredFields &fields, + std::string_view text) { + if (!elements.empty() && elements.back().required_index >= 0) { + fields[static_cast(elements.back().required_index)].append(text); + } +} + +RequiredFields extract_required_fields(std::string_view xml) { + RequiredFields fields; + std::array seen{}; + std::vector elements; + std::size_t position{}; + + while (position < xml.size()) { + const auto markup = xml.find('<', position); + if (markup == std::string_view::npos) { + append_required_text(elements, fields, xml.substr(position)); + position = xml.size(); + break; + } + append_required_text(elements, fields, xml.substr(position, markup - position)); + + if (xml.substr(markup, 4) == "", markup + 4); + if (end == std::string_view::npos) { + throw DiscoveryError("sensor configuration contains an unterminated comment"); + } + position = end + 3; + continue; + } + if (xml.substr(markup, 9) == "", markup + 9); + if (end == std::string_view::npos) { + throw DiscoveryError("sensor configuration contains unterminated CDATA"); + } + append_required_text(elements, fields, xml.substr(markup + 9, end - markup - 9)); + position = end + 3; + continue; + } + if (xml.substr(markup, 2) == "", markup + 2); + if (end == std::string_view::npos) { + throw DiscoveryError("sensor configuration contains unterminated markup"); + } + position = end + 2; + continue; + } + if (xml.substr(markup, 2) == "= 0) { + throw DiscoveryError("sensor configuration fields must contain text only"); + } + if (required_index >= 0) { + const auto index = static_cast(required_index); + if (seen[index]) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' must appear exactly once"); + } + seen[index] = true; + } + if (!self_closing) { + elements.push_back(OpenElement{name, required_index}); + } + position = end + 1; + } + + if (!elements.empty()) { + throw DiscoveryError("sensor configuration contains malformed element nesting"); + } + for (std::size_t index = 0; index < kRequiredTags.size(); ++index) { + if (!seen[index]) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' must appear exactly once"); + } + const auto value = trim_ascii_whitespace(fields[index]); + if (value.empty()) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' is empty"); + } + if (value.size() > kMaximumFieldLength) { + throw DiscoveryError("sensor configuration field '" + std::string{kRequiredTags[index]} + + "' is too long"); + } + fields[index] = std::string{value}; + } + return fields; +} + +double parse_positive_count(std::string_view value, std::string_view tag) { + double result{}; + const auto parsed = std::from_chars(value.data(), value.data() + value.size(), result, + std::chars_format::general); + if (parsed.ec != std::errc{} || parsed.ptr != value.data() + value.size() || + !std::isfinite(result) || result <= 0.0) { + throw DiscoveryError("sensor configuration field '" + std::string{tag} + + "' must be a finite positive number"); + } + return result; +} + +std::string_view normalize_torque_spelling(std::string_view value) noexcept { + if (value == "Nm") { + return "N-m"; + } + if (value == "Nmm") { + return "N-mm"; + } + if (value == "kg-cm") { + return "kgf-cm"; + } + return value; +} + +ForceUnit parse_force_unit(std::string_view value) { + const auto unit = force_unit_from_string(value); + if (!unit.has_value() || *unit == ForceUnit::Unknown) { + throw DiscoveryError("sensor configuration contains an unknown force unit"); + } + return *unit; +} + +TorqueUnit parse_torque_unit(std::string_view value) { + const auto unit = torque_unit_from_string(normalize_torque_spelling(value)); + if (!unit.has_value() || *unit == TorqueUnit::Unknown) { + throw DiscoveryError("sensor configuration contains an unknown torque unit"); + } + return *unit; +} + +} // namespace + +SensorConfiguration parse_sensor_configuration(std::string_view xml) { + const auto fields = extract_required_fields(xml); + SensorConfiguration configuration; + configuration.product_name = fields[0]; + configuration.calibration.counts_per_force_unit = parse_positive_count(fields[1], "cfgcpf"); + configuration.calibration.counts_per_torque_unit = parse_positive_count(fields[2], "cfgcpt"); + configuration.calibration.force_unit = parse_force_unit(fields[3]); + configuration.calibration.torque_unit = parse_torque_unit(fields[4]); + configuration.source = CalibrationSource::Sensor; + configuration.revision = 1; + return configuration; +} + +} // namespace netft::detail diff --git a/src/core/src/detail/xml_config.hpp b/src/core/src/detail/xml_config.hpp new file mode 100644 index 0000000..c9b653f --- /dev/null +++ b/src/core/src/detail/xml_config.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include + +#include "netft/types.hpp" + +namespace netft::detail { + +SensorConfiguration parse_sensor_configuration(std::string_view xml); + +} // namespace netft::detail diff --git a/src/core/src/discovery.cpp b/src/core/src/discovery.cpp new file mode 100644 index 0000000..cd507f2 --- /dev/null +++ b/src/core/src/discovery.cpp @@ -0,0 +1,183 @@ +#include "netft/discovery.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "detail/xml_config.hpp" + +namespace netft { +namespace { + +constexpr std::size_t kMaximumResponseBytes = 65'536; + +struct CurlHandleDeleter { + void operator()(CURL *handle) const noexcept { curl_easy_cleanup(handle); } +}; + +using CurlHandle = std::unique_ptr; + +struct CurlUrlDeleter { + void operator()(CURLU *url) const noexcept { curl_url_cleanup(url); } +}; + +using CurlUrl = std::unique_ptr; + +struct ResponseBuffer { + std::string body; + bool too_large{false}; + bool write_failed{false}; +}; + +bool is_ascii_blank(std::string_view value) noexcept { + if (value.empty()) { + return true; + } + return std::all_of(value.begin(), value.end(), [](const char character) { + return character == ' ' || character == '\t' || character == '\n' || character == '\r' || + character == '\f' || character == '\v'; + }); +} + +long timeout_milliseconds(std::chrono::duration timeout, std::string_view name) { + const double seconds = timeout.count(); + if (!std::isfinite(seconds) || seconds <= 0.0) { + throw DiscoveryError(std::string{name} + " must be finite, positive, and representable"); + } + + const long double rounded_milliseconds = std::ceil(static_cast(seconds) * 1000.0L); + if (!std::isfinite(rounded_milliseconds) || + rounded_milliseconds > static_cast(std::numeric_limits::max())) { + throw DiscoveryError(std::string{name} + " must be finite, positive, and representable"); + } + return static_cast(rounded_milliseconds); +} + +void validate_options(const DiscoveryOptions &options) { + if (is_ascii_blank(options.sensor_host)) { + throw DiscoveryError("sensor_host must not be blank"); + } + if (options.http_port < 1 || options.http_port > 65'535) { + throw DiscoveryError("http_port must be in the range 1..65535"); + } + static_cast(timeout_milliseconds(options.connect_timeout, "connect_timeout")); + static_cast(timeout_milliseconds(options.total_timeout, "total_timeout")); +} + +CURLcode initialize_curl_once() { + static std::once_flag initialization_flag; + static CURLcode initialization_result = CURLE_FAILED_INIT; + std::call_once(initialization_flag, + [] { initialization_result = curl_global_init(CURL_GLOBAL_DEFAULT); }); + return initialization_result; +} + +template void set_curl_option(CURL *handle, CURLoption option, Value value) { + if (curl_easy_setopt(handle, option, value) != CURLE_OK) { + throw DiscoveryError("failed to configure sensor discovery request"); + } +} + +void set_url_part(CURLU *url, CURLUPart part, const char *value) { + if (curl_url_set(url, part, value, 0) != CURLUE_OK) { + throw DiscoveryError("invalid sensor discovery URL"); + } +} + +std::size_t append_response(char *data, std::size_t size, std::size_t count, + void *user_data) noexcept { + auto &response = *static_cast(user_data); + if (size != 0 && count > std::numeric_limits::max() / size) { + response.too_large = true; + return 0; + } + const std::size_t byte_count = size * count; + if (byte_count > kMaximumResponseBytes - response.body.size()) { + response.too_large = true; + return 0; + } + try { + response.body.append(data, byte_count); + } catch (...) { + response.write_failed = true; + return 0; + } + return byte_count; +} + +} // namespace + +SensorConfiguration discover_sensor(const DiscoveryOptions &options) { + validate_options(options); + if (initialize_curl_once() != CURLE_OK) { + throw DiscoveryError("failed to initialize HTTP transport"); + } + + CurlHandle handle{curl_easy_init()}; + if (!handle) { + throw DiscoveryError("failed to create HTTP transport"); + } + + CurlUrl url{curl_url()}; + if (!url) { + throw DiscoveryError("failed to create sensor discovery URL"); + } + std::string host = options.sensor_host; + if (host.find(':') != std::string::npos && + (host.size() < 2 || host.front() != '[' || host.back() != ']')) { + host = "[" + host + "]"; + } + const std::string port = std::to_string(options.http_port); + set_url_part(url.get(), CURLUPART_SCHEME, "http"); + set_url_part(url.get(), CURLUPART_HOST, host.c_str()); + set_url_part(url.get(), CURLUPART_PORT, port.c_str()); + set_url_part(url.get(), CURLUPART_PATH, "/netftapi2.xml"); + + ResponseBuffer response; + set_curl_option(handle.get(), CURLOPT_CURLU, url.get()); +#if LIBCURL_VERSION_NUM >= 0x075500 + set_curl_option(handle.get(), CURLOPT_PROTOCOLS_STR, "http"); +#else + set_curl_option(handle.get(), CURLOPT_PROTOCOLS, static_cast(CURLPROTO_HTTP)); +#endif + set_curl_option(handle.get(), CURLOPT_FOLLOWLOCATION, 0L); + set_curl_option(handle.get(), CURLOPT_MAXREDIRS, 0L); + set_curl_option(handle.get(), CURLOPT_NOSIGNAL, 1L); + set_curl_option(handle.get(), CURLOPT_PROXY, ""); + set_curl_option(handle.get(), CURLOPT_CONNECTTIMEOUT_MS, + timeout_milliseconds(options.connect_timeout, "connect_timeout")); + set_curl_option(handle.get(), CURLOPT_TIMEOUT_MS, + timeout_milliseconds(options.total_timeout, "total_timeout")); + set_curl_option(handle.get(), CURLOPT_WRITEFUNCTION, &append_response); + set_curl_option(handle.get(), CURLOPT_WRITEDATA, &response); + + const CURLcode transfer_result = curl_easy_perform(handle.get()); + if (response.too_large) { + throw DiscoveryError("sensor configuration response exceeds 65536 bytes"); + } + if (response.write_failed) { + throw DiscoveryError("failed to store sensor configuration response"); + } + if (transfer_result != CURLE_OK) { + throw DiscoveryError(std::string{"sensor configuration request failed: "} + + curl_easy_strerror(transfer_result)); + } + + long status{}; + if (curl_easy_getinfo(handle.get(), CURLINFO_RESPONSE_CODE, &status) != CURLE_OK) { + throw DiscoveryError("failed to inspect sensor configuration response"); + } + if (status != 200) { + throw DiscoveryError("sensor configuration request returned HTTP " + std::to_string(status)); + } + + return detail::parse_sensor_configuration(response.body); +} + +} // namespace netft diff --git a/src/core/src/status.cpp b/src/core/src/status.cpp new file mode 100644 index 0000000..344f0a9 --- /dev/null +++ b/src/core/src/status.cpp @@ -0,0 +1,68 @@ +#include "netft/status.hpp" + +#include +#include + +namespace netft { +namespace { + +constexpr std::array, 30> kStatusBits{{ + {0x80000000U, "error summary"}, + {0x40000000U, "CPU or RAM error"}, + {0x20000000U, "digital board error"}, + {0x10000000U, "analog board error"}, + {0x08000000U, "serial link communication error"}, + {0x04000000U, "program memory verification error"}, + {0x02000000U, "halted due to configuration errors"}, + {0x01000000U, "settings validation error"}, + {0x00800000U, "configuration incompatible with calibration"}, + {0x00400000U, "network communication failure"}, + {0x00200000U, "CAN communication error"}, + {0x00100000U, "RDT communication error"}, + {0x00080000U, "EtherNet/IP protocol failure"}, + {0x00040000U, "DeviceNet protocol failure"}, + {0x00020000U, "transducer saturation or A/D error"}, + {0x00010000U, "monitor condition latched"}, + {0x00004000U, "watchdog timeout error"}, + {0x00002000U, "stack check error"}, + {0x00001000U, "serial EEPROM I2C failure"}, + {0x00000800U, "serial flash SPI failure"}, + {0x00000400U, "analog board watchdog timeout"}, + {0x00000200U, "excessive strain gage excitation current"}, + {0x00000100U, "insufficient strain gage excitation current"}, + {0x00000080U, "artificial analog ground out of range"}, + {0x00000040U, "analog board power supply too high"}, + {0x00000020U, "analog board power supply too low"}, + {0x00000010U, "serial link data unavailable"}, + {0x00000008U, "reference voltage or power monitoring error"}, + {0x00000004U, "internal temperature error"}, + {0x00000002U, "HTTP protocol failure"}, +}}; + +} // namespace + +StatusSeverity classify_status(const std::uint32_t status) noexcept { + if (status == 0) { + return StatusSeverity::Ok; + } + return status == 0x80010000U ? StatusSeverity::Warn : StatusSeverity::Error; +} + +std::string decode_status(const std::uint32_t status) { + if (status == 0) { + return "healthy"; + } + + std::string result; + for (const auto &[mask, name] : kStatusBits) { + if ((status & mask) != 0) { + if (!result.empty()) { + result += ", "; + } + result += name; + } + } + return result; +} + +} // namespace netft diff --git a/src/core/src/types.cpp b/src/core/src/types.cpp new file mode 100644 index 0000000..3f2824b --- /dev/null +++ b/src/core/src/types.cpp @@ -0,0 +1,164 @@ +#include "netft/types.hpp" + +#include +#include +#include +#include + +namespace netft { +namespace { + +bool is_blank(std::string_view value) { + return value.empty() || std::all_of(value.begin(), value.end(), [](unsigned char character) { + return std::isspace(character) != 0; + }); +} + +void require_positive_finite(std::string_view name, double value) { + if (!std::isfinite(value) || value <= 0.0) { + throw std::invalid_argument(std::string{name} + " must be finite and positive"); + } +} + +void require_port(std::string_view name, int value) { + if (value < 1 || value > 65535) { + throw std::invalid_argument(std::string{name} + " must be in the range 1..65535"); + } +} + +} // namespace + +void validate(const Calibration &calibration) { + require_positive_finite("counts_per_force_unit", calibration.counts_per_force_unit); + require_positive_finite("counts_per_torque_unit", calibration.counts_per_torque_unit); + if (calibration.force_unit == ForceUnit::Unknown || + calibration.torque_unit == TorqueUnit::Unknown) { + throw std::invalid_argument("calibration units must be known"); + } +} + +void validate(const Config &config) { + if (is_blank(config.sensor_host)) { + throw std::invalid_argument("sensor_host must not be blank"); + } + require_port("rdt_port", config.rdt_port); + require_port("http_port", config.http_port); + require_positive_finite("receive_timeout", config.receive_timeout.count()); + require_positive_finite("configuration_connect_timeout", + config.configuration_connect_timeout.count()); + require_positive_finite("configuration_timeout", config.configuration_timeout.count()); + require_positive_finite("reconnect_initial_delay", config.reconnect_initial_delay.count()); + require_positive_finite("reconnect_max_delay", config.reconnect_max_delay.count()); + if (config.reconnect_max_delay < config.reconnect_initial_delay) { + throw std::invalid_argument("reconnect_max_delay must not be below reconnect_initial_delay"); + } + if (!std::isfinite(config.sample_rate_limit_hz) || config.sample_rate_limit_hz < 0.0) { + throw std::invalid_argument("sample_rate_limit_hz must be finite and non-negative"); + } + if (config.calibration_override.has_value()) { + validate(*config.calibration_override); + } +} + +std::string_view to_string(ForceUnit unit) noexcept { + switch (unit) { + case ForceUnit::PoundForce: + return "lbf"; + case ForceUnit::Newton: + return "N"; + case ForceUnit::KiloPoundForce: + return "klbf"; + case ForceUnit::KiloNewton: + return "kN"; + case ForceUnit::KilogramForce: + return "kgf"; + case ForceUnit::Unknown: + return "unknown"; + } + return "unknown"; +} + +std::string_view to_string(TorqueUnit unit) noexcept { + switch (unit) { + case TorqueUnit::PoundForceInch: + return "lbf-in"; + case TorqueUnit::PoundForceFoot: + return "lbf-ft"; + case TorqueUnit::NewtonMeter: + return "N-m"; + case TorqueUnit::NewtonMillimeter: + return "N-mm"; + case TorqueUnit::KilogramForceCentimeter: + return "kgf-cm"; + case TorqueUnit::KiloNewtonMeter: + return "kN-m"; + case TorqueUnit::Unknown: + return "unknown"; + } + return "unknown"; +} + +std::optional force_unit_from_string(std::string_view value) noexcept { + for (const auto unit : + {ForceUnit::Unknown, ForceUnit::PoundForce, ForceUnit::Newton, ForceUnit::KiloPoundForce, + ForceUnit::KiloNewton, ForceUnit::KilogramForce}) { + if (to_string(unit) == value) { + return unit; + } + } + return std::nullopt; +} + +std::optional torque_unit_from_string(std::string_view value) noexcept { + for (const auto unit : + {TorqueUnit::Unknown, TorqueUnit::PoundForceInch, TorqueUnit::PoundForceFoot, + TorqueUnit::NewtonMeter, TorqueUnit::NewtonMillimeter, TorqueUnit::KilogramForceCentimeter, + TorqueUnit::KiloNewtonMeter}) { + if (to_string(unit) == value) { + return unit; + } + } + return std::nullopt; +} + +std::string_view to_string(ClientState state) noexcept { + switch (state) { + case ClientState::Stopped: + return "stopped"; + case ClientState::Connecting: + return "connecting"; + case ClientState::Streaming: + return "streaming"; + case ClientState::Backoff: + return "backoff"; + case ClientState::Faulted: + return "faulted"; + } + return "unknown"; +} + +std::string_view to_string(FaultCode code) noexcept { + switch (code) { + case FaultCode::None: + return "none"; + case FaultCode::SensorConfiguration: + return "sensor_configuration"; + case FaultCode::Timeout: + return "timeout"; + case FaultCode::Socket: + return "socket"; + case FaultCode::SeriousStatus: + return "serious_status"; + case FaultCode::FtStall: + return "ft_stall"; + case FaultCode::FtBackward: + return "ft_backward"; + case FaultCode::MalformedStorm: + return "malformed_storm"; + case FaultCode::Callback: + return "callback"; + } + return "unknown"; +} + +} // namespace netft diff --git a/src/netft_hardware_interface.cpp b/src/netft_hardware_interface.cpp index 1d47979..a1b09b7 100644 --- a/src/netft_hardware_interface.cpp +++ b/src/netft_hardware_interface.cpp @@ -1,7 +1,8 @@ -#include "netft_driver/ros2_control_compat.hpp" +#include "ros/diagnostics.hpp" +#include "ros/ros2_control_compat.hpp" +#include "ros/unit_conversion.hpp" -#include "netft_driver/client.hpp" -#include "netft_driver/status.hpp" +#include "netft/client.hpp" #include #include @@ -30,7 +31,15 @@ #include #ifdef NETFT_ROS2_CONTROL_TESTING -#include +#include "ros/ros2_control_test_access.hpp" + +#if defined(_WIN32) +#define NETFT_ROS2_CONTROL_TEST_EXPORT __declspec(dllexport) +#elif defined(__GNUC__) || defined(__clang__) +#define NETFT_ROS2_CONTROL_TEST_EXPORT __attribute__((visibility("default"))) +#else +#define NETFT_ROS2_CONTROL_TEST_EXPORT +#endif #endif namespace netft_driver { @@ -38,17 +47,21 @@ namespace netft_driver { class NetFTHardwareInterface; #ifdef NETFT_ROS2_CONTROL_TESTING -struct NetFTClientTestAccess { - static void break_socket(NetFTClient & client) noexcept - { - std::lock_guard lock{client.socket_mutex_}; - if (client.socket_ >= 0) { - (void)::close(client.socket_); - client.socket_ = -1; - client.session_started_ = false; - } - } -}; +namespace ros2_control_test_access::detail { + +void force_initial_sample_once() noexcept; +void fail_state_write_once_at(std::size_t axis) noexcept; +void latch_active_fault(FaultCode fault) noexcept; +bool read_active_instance() noexcept; +FaultCode active_fault_code() noexcept; +FaultCode active_client_fault_code() noexcept; +FaultCode active_latched_fault_code() noexcept; +ActivityCounters active_activity_counters() noexcept; +bool interface_write_fault_latched() noexcept; +void throw_executor_cancel_once() noexcept; +int auxiliary_thread_count() noexcept; + +} // namespace ros2_control_test_access::detail #endif namespace { @@ -64,7 +77,7 @@ std::atomic g_test_return_initial_sample{false}; std::atomic g_test_failed_write_axis{-1}; std::atomic g_test_throw_executor_cancel{false}; std::atomic g_test_auxiliary_threads{0}; -const WrenchSample kInitialSample{}; +const SiSample kInitialSample{}; #endif double parse_double( @@ -87,9 +100,9 @@ double parse_double( int parse_port( const std::unordered_map & parameters, - int fallback) + const char * field, int fallback) { - const auto found = parameters.find("sensor_port"); + const auto found = parameters.find(field); if (found == parameters.end()) return fallback; try { std::size_t consumed = 0; @@ -99,10 +112,22 @@ int parse_port( } return static_cast(value); } catch (const std::exception &) { - throw std::invalid_argument{"sensor_port must be an integer between 1 and 65535"}; + throw std::invalid_argument{ + std::string{field} + " must be an integer between 1 and 65535"}; } } +bool parse_bool( + const std::unordered_map & parameters, + const char * field, bool fallback) +{ + const auto found = parameters.find(field); + if (found == parameters.end()) return fallback; + if (found->second == "true" || found->second == "True") return true; + if (found->second == "false" || found->second == "False") return false; + throw std::invalid_argument{std::string{field} + " must be true or false"}; +} + std::int64_t steady_nanoseconds(std::chrono::steady_clock::time_point time) noexcept { return std::chrono::duration_cast( @@ -212,11 +237,11 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface client_.reset(); sample_generation_.store(0, std::memory_order_release); interface_write_fault_.store(false, std::memory_order_release); - fatal_fault_.store(FaultCode::None, std::memory_order_release); - sample_buffer_.initRT(WrenchSample{}); + fatal_fault_.store(netft::FaultCode::None, std::memory_order_release); + sample_buffer_.initRT(SiSample{}); invalidate_interfaces(); try { - client_ = std::make_unique(client_config_); + client_ = std::make_unique(client_config_); start_auxiliary(); return hardware_interface::CallbackReturn::SUCCESS; } catch (const std::exception &) { @@ -244,8 +269,8 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface sample_generation_.store(0, std::memory_order_release); invalidate_interfaces(); try { - client_->start([this](const WrenchSample & sample) { - sample_buffer_.writeFromNonRT(sample); + client_->start([this](const netft::Sample & sample) { + sample_buffer_.writeFromNonRT(to_si_sample(sample)); sample_generation_.fetch_add(1, std::memory_order_release); }); } catch (const std::exception &) { @@ -296,7 +321,7 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface hardware_interface::return_type read( const rclcpp::Time &, const rclcpp::Duration &) override { - const WrenchSample * sample = nullptr; + const SiSample * sample = nullptr; #ifdef NETFT_ROS2_CONTROL_TESTING if (g_test_return_initial_sample.exchange(false, std::memory_order_acq_rel)) { sample = &kInitialSample; @@ -312,7 +337,7 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface return hardware_interface::return_type::ERROR; } if (sample_is_stale(*sample)) { - latch_fatal_fault(FaultCode::Timeout); + latch_fatal_fault(netft::FaultCode::Timeout); invalidate_interfaces(); return hardware_interface::return_type::ERROR; } @@ -326,9 +351,17 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface private: #ifdef NETFT_ROS2_CONTROL_TESTING - friend FaultCode ros2_control_compat::test_active_fault_code() noexcept; - friend bool ros2_control_compat::test_interface_write_fault_latched() noexcept; - friend void ros2_control_compat::test_break_active_socket() noexcept; + friend ros2_control_test_access::FaultCode + ros2_control_test_access::detail::active_fault_code() noexcept; + friend ros2_control_test_access::FaultCode + ros2_control_test_access::detail::active_client_fault_code() noexcept; + friend ros2_control_test_access::FaultCode + ros2_control_test_access::detail::active_latched_fault_code() noexcept; + friend ros2_control_test_access::ActivityCounters + ros2_control_test_access::detail::active_activity_counters() noexcept; + friend bool ros2_control_test_access::detail::interface_write_fault_latched() noexcept; + friend void ros2_control_test_access::detail::latch_active_fault( + ros2_control_test_access::FaultCode) noexcept; #endif void validate_hardware_info(const hardware_interface::HardwareInfo & info) @@ -382,21 +415,31 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface void parse_parameters( const std::unordered_map & parameters) { - ClientConfig config; + netft::Config config; const auto host = parameters.find("sensor_ip"); if (host != parameters.end()) config.sensor_host = host->second; if (config.sensor_host.find_first_not_of(" \t\n\r\f\v") == std::string::npos) { throw std::invalid_argument{"sensor_ip must be non-empty"}; } - config.sensor_port = parse_port(parameters, config.sensor_port); - config.counts_per_force = parse_double( - parameters, "counts_per_force", config.counts_per_force); - config.counts_per_torque = parse_double( - parameters, "counts_per_torque", config.counts_per_torque); + config.rdt_port = parse_port(parameters, "sensor_port", config.rdt_port); + config.http_port = parse_port(parameters, "http_port", config.http_port); config.receive_timeout = std::chrono::duration{parse_double( parameters, "receive_timeout", config.receive_timeout.count())}; - config.recovery_policy = RecoveryPolicy::FailStop; - validate(config); + config.configuration_connect_timeout = std::chrono::duration{parse_double( + parameters, "configuration_connect_timeout", + config.configuration_connect_timeout.count())}; + config.configuration_timeout = std::chrono::duration{parse_double( + parameters, "configuration_timeout", config.configuration_timeout.count())}; + if (!parse_bool(parameters, "use_sensor_calibration", true)) { + config.calibration_override = netft::Calibration{ + parse_double(parameters, "counts_per_force", 1000000.0), + parse_double(parameters, "counts_per_torque", 1000000.0), + netft::ForceUnit::Newton, + netft::TorqueUnit::NewtonMeter, + }; + } + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::validate(config); activation_timeout_ = std::chrono::duration{parse_double( parameters, "activation_timeout", 2.0)}; @@ -443,7 +486,7 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface [this](const std::shared_ptr, std::shared_ptr response) { try { - if (!client_) throw NotConnectedError{"client is not configured"}; + if (!client_) throw std::runtime_error{"client is not configured"}; client_->bias(); response->success = true; response->message = "software bias command sent and RDT streaming restarted"; @@ -506,12 +549,13 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface void publish_diagnostics() { if (!client_ || !evaluator_ || !diagnostics_publisher_) return; - auto snapshot = client_->health_snapshot(); + auto snapshot = client_->health(); const auto plugin_fault = fatal_fault_.load(std::memory_order_acquire); - if (plugin_fault != FaultCode::None && snapshot.fault_code == FaultCode::None) { - snapshot.state = ClientState::Faulted; + if (plugin_fault != netft::FaultCode::None && + snapshot.fault_code == netft::FaultCode::None) { + snapshot.state = netft::ClientState::Faulted; snapshot.fault_code = plugin_fault; - if (snapshot.last_error.empty() && plugin_fault == FaultCode::Timeout) { + if (snapshot.last_error.empty() && plugin_fault == netft::FaultCode::Timeout) { snapshot.last_error = "no valid RDT record before timeout"; } } @@ -520,7 +564,7 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface status.level = static_cast(report.level); status.name = "netft_driver: " + sensor_name_; status.message = report.message; - status.hardware_id = snapshot.sensor_host + ":" + std::to_string(snapshot.sensor_port); + status.hardware_id = snapshot.sensor_host + ":" + std::to_string(snapshot.rdt_port); status.values.reserve(report.values.size()); for (const auto & value : report.values) { diagnostic_msgs::msg::KeyValue item; @@ -534,9 +578,9 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface diagnostics_publisher_->publish(message); } - void latch_fatal_fault(const FaultCode fault) noexcept + void latch_fatal_fault(const netft::FaultCode fault) noexcept { - auto expected = FaultCode::None; + auto expected = netft::FaultCode::None; (void)fatal_fault_.compare_exchange_strong( expected, fault, std::memory_order_acq_rel); } @@ -544,13 +588,13 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface bool fault_latched() noexcept { if (!client_) return true; - if (client_->faulted() && client_->fault_code() != FaultCode::None) { + if (client_->faulted() && client_->fault_code() != netft::FaultCode::None) { latch_fatal_fault(client_->fault_code()); } - return fatal_fault_.load(std::memory_order_acquire) != FaultCode::None; + return fatal_fault_.load(std::memory_order_acquire) != netft::FaultCode::None; } - bool sample_is_stale(const WrenchSample & sample) const noexcept + bool sample_is_stale(const SiSample & sample) const noexcept { const auto received_ns = steady_nanoseconds(sample.received_at); const auto now_ns = steady_nanoseconds(std::chrono::steady_clock::now()); @@ -559,7 +603,7 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface return received_ns <= 0 || now_ns - received_ns > timeout_ns; } - bool write_axes(const WrenchSample & sample) noexcept + bool write_axes(const SiSample & sample) noexcept { const std::array values{ sample.force[0], sample.force[1], sample.force[2], @@ -600,7 +644,7 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface invalidate_interfaces(); } - ClientConfig client_config_{}; + netft::Config client_config_{}; std::chrono::duration activation_timeout_{2.0}; double diagnostics_rate_{1.0}; double expected_rdt_rate_{2000.0}; @@ -608,7 +652,7 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface std::string sensor_name_; std::string ros_name_token_; std::string bias_service_name_; - std::unique_ptr client_; + std::unique_ptr client_; std::unique_ptr evaluator_; std::shared_ptr auxiliary_node_; std::unique_ptr auxiliary_executor_; @@ -617,10 +661,10 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface rclcpp::TimerBase::SharedPtr diagnostic_timer_; std::thread auxiliary_thread_; std::atomic auxiliary_stopping_{false}; - realtime_tools::RealtimeBuffer sample_buffer_; + realtime_tools::RealtimeBuffer sample_buffer_; std::atomic sample_generation_{0}; std::atomic interface_write_fault_{false}; - std::atomic fatal_fault_{FaultCode::None}; + std::atomic fatal_fault_{netft::FaultCode::None}; #ifdef NETFT_ROS2_CONTROL_LEGACY_API std::array state_values_{}; #else @@ -629,30 +673,63 @@ class NetFTHardwareInterface final : public hardware_interface::SensorInterface }; static_assert(std::atomic::is_always_lock_free); -static_assert(std::atomic::is_always_lock_free); +static_assert(std::atomic::is_always_lock_free); #ifdef NETFT_ROS2_CONTROL_TESTING -namespace ros2_control_compat { +namespace ros2_control_test_access::detail { +namespace { + +FaultCode to_test_fault(const netft::FaultCode fault) noexcept +{ + switch (fault) { + case netft::FaultCode::None: return FaultCode::None; + case netft::FaultCode::SensorConfiguration: return FaultCode::SensorConfiguration; + case netft::FaultCode::Timeout: return FaultCode::Timeout; + case netft::FaultCode::Socket: return FaultCode::Socket; + case netft::FaultCode::SeriousStatus: return FaultCode::SeriousStatus; + case netft::FaultCode::FtStall: return FaultCode::FtStall; + case netft::FaultCode::FtBackward: return FaultCode::FtBackward; + case netft::FaultCode::MalformedStorm: return FaultCode::MalformedStorm; + case netft::FaultCode::Callback: return FaultCode::Callback; + } + return FaultCode::None; +} -void test_force_initial_sample_once() noexcept +netft::FaultCode to_client_fault(const FaultCode fault) noexcept +{ + switch (fault) { + case FaultCode::None: return netft::FaultCode::None; + case FaultCode::SensorConfiguration: return netft::FaultCode::SensorConfiguration; + case FaultCode::Timeout: return netft::FaultCode::Timeout; + case FaultCode::Socket: return netft::FaultCode::Socket; + case FaultCode::SeriousStatus: return netft::FaultCode::SeriousStatus; + case FaultCode::FtStall: return netft::FaultCode::FtStall; + case FaultCode::FtBackward: return netft::FaultCode::FtBackward; + case FaultCode::MalformedStorm: return netft::FaultCode::MalformedStorm; + case FaultCode::Callback: return netft::FaultCode::Callback; + } + return netft::FaultCode::None; +} + +} // namespace + +void force_initial_sample_once() noexcept { g_test_return_initial_sample.store(true, std::memory_order_release); } -void test_fail_state_write_once_at(const std::size_t axis) noexcept +void fail_state_write_once_at(const std::size_t axis) noexcept { g_test_failed_write_axis.store(static_cast(axis), std::memory_order_release); } -void test_break_active_socket() noexcept +void latch_active_fault(const FaultCode fault) noexcept { auto * instance = g_test_instance.load(std::memory_order_acquire); - if (instance != nullptr && instance->client_) { - NetFTClientTestAccess::break_socket(*instance->client_); - } + if (instance != nullptr) instance->latch_fatal_fault(to_client_fault(fault)); } -bool test_read_active_instance() noexcept +bool read_active_instance() noexcept { auto * instance = g_test_instance.load(std::memory_order_acquire); if (instance == nullptr) return false; @@ -661,36 +738,131 @@ bool test_read_active_instance() noexcept hardware_interface::return_type::OK; } -FaultCode test_active_fault_code() noexcept +FaultCode active_fault_code() noexcept { const auto * instance = g_test_instance.load(std::memory_order_acquire); if (instance == nullptr) return FaultCode::None; const auto latched = instance->fatal_fault_.load(std::memory_order_acquire); - if (latched != FaultCode::None || !instance->client_) return latched; - return instance->client_->fault_code(); + if (latched != netft::FaultCode::None || !instance->client_) { + return to_test_fault(latched); + } + return to_test_fault(instance->client_->fault_code()); +} + +FaultCode active_client_fault_code() noexcept +{ + const auto * instance = g_test_instance.load(std::memory_order_acquire); + if (instance == nullptr || !instance->client_) return FaultCode::None; + return to_test_fault(instance->client_->fault_code()); +} + +FaultCode active_latched_fault_code() noexcept +{ + const auto * instance = g_test_instance.load(std::memory_order_acquire); + if (instance == nullptr) return FaultCode::None; + return to_test_fault(instance->fatal_fault_.load(std::memory_order_acquire)); +} + +ActivityCounters active_activity_counters() noexcept +{ + const auto * instance = g_test_instance.load(std::memory_order_acquire); + if (instance == nullptr) return {}; + return { + instance->sample_generation_.load(std::memory_order_acquire), + }; } -bool test_interface_write_fault_latched() noexcept +bool interface_write_fault_latched() noexcept { const auto * instance = g_test_instance.load(std::memory_order_acquire); return instance != nullptr && instance->interface_write_fault_.load(std::memory_order_acquire); } -void test_throw_executor_cancel_once() noexcept +void throw_executor_cancel_once() noexcept { g_test_throw_executor_cancel.store(true, std::memory_order_release); } -int test_auxiliary_thread_count() noexcept +int auxiliary_thread_count() noexcept { return g_test_auxiliary_threads.load(std::memory_order_acquire); } -} // namespace ros2_control_compat +} // namespace ros2_control_test_access::detail #endif } // namespace netft_driver +#ifdef NETFT_ROS2_CONTROL_TESTING +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT void +netft_ros2_control_test_force_initial_sample_once() noexcept +{ + netft_driver::ros2_control_test_access::detail::force_initial_sample_once(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT void netft_ros2_control_test_fail_state_write_once_at( + const std::size_t axis) noexcept +{ + netft_driver::ros2_control_test_access::detail::fail_state_write_once_at(axis); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT void netft_ros2_control_test_latch_active_fault( + const netft_driver::ros2_control_test_access::FaultCode fault) noexcept +{ + netft_driver::ros2_control_test_access::detail::latch_active_fault(fault); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT bool +netft_ros2_control_test_read_active_instance() noexcept +{ + return netft_driver::ros2_control_test_access::detail::read_active_instance(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT netft_driver::ros2_control_test_access::FaultCode +netft_ros2_control_test_active_fault_code() noexcept +{ + return netft_driver::ros2_control_test_access::detail::active_fault_code(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT netft_driver::ros2_control_test_access::FaultCode +netft_ros2_control_test_active_client_fault_code() noexcept +{ + return netft_driver::ros2_control_test_access::detail::active_client_fault_code(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT netft_driver::ros2_control_test_access::FaultCode +netft_ros2_control_test_active_latched_fault_code() noexcept +{ + return netft_driver::ros2_control_test_access::detail::active_latched_fault_code(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT netft_driver::ros2_control_test_access::ActivityCounters +netft_ros2_control_test_active_activity_counters() noexcept +{ + return netft_driver::ros2_control_test_access::detail::active_activity_counters(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT bool +netft_ros2_control_test_interface_write_fault_latched() noexcept +{ + return netft_driver::ros2_control_test_access::detail::interface_write_fault_latched(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT void +netft_ros2_control_test_throw_executor_cancel_once() noexcept +{ + netft_driver::ros2_control_test_access::detail::throw_executor_cancel_once(); +} + +extern "C" NETFT_ROS2_CONTROL_TEST_EXPORT int +netft_ros2_control_test_auxiliary_thread_count() noexcept +{ + return netft_driver::ros2_control_test_access::detail::auxiliary_thread_count(); +} + +#undef NETFT_ROS2_CONTROL_TEST_EXPORT +#endif + PLUGINLIB_EXPORT_CLASS( netft_driver::NetFTHardwareInterface, hardware_interface::SensorInterface) diff --git a/src/protocol.cpp b/src/protocol.cpp deleted file mode 100644 index 1551b62..0000000 --- a/src/protocol.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "netft_driver/protocol.hpp" - -#include - -namespace netft_driver { -namespace { - -std::uint32_t read_u32(const std::uint8_t * data) -{ - return (static_cast(data[0]) << 24U) | - (static_cast(data[1]) << 16U) | - (static_cast(data[2]) << 8U) | - static_cast(data[3]); -} - -std::int32_t read_i32(const std::uint8_t * data) -{ - const std::uint32_t value = read_u32(data); - if (value <= 0x7fffffffU) { - return static_cast(value); - } - return -static_cast(~value) - 1; -} - -} // namespace - -std::array encode_request(const Command command, const std::uint32_t count) -{ - const auto value = static_cast(command); - return {{0x12, 0x34, - static_cast(value >> 8U), static_cast(value), - static_cast(count >> 24U), static_cast(count >> 16U), - static_cast(count >> 8U), static_cast(count)}}; -} - -RawRecord decode_record(const std::uint8_t * data, const std::size_t size) -{ - if (size != 36) { - throw ProtocolError{"RDT record must be exactly 36 bytes, received " + std::to_string(size)}; - } - if (data == nullptr) { - throw ProtocolError{"RDT record data must not be null"}; - } - return {read_u32(data), read_u32(data + 4), read_u32(data + 8), - read_i32(data + 12), read_i32(data + 16), read_i32(data + 20), - read_i32(data + 24), read_i32(data + 28), read_i32(data + 32)}; -} - -} // namespace netft_driver diff --git a/include/netft_driver/adapter_config.hpp b/src/ros/adapter_config.hpp similarity index 73% rename from include/netft_driver/adapter_config.hpp rename to src/ros/adapter_config.hpp index 4a55943..7185b7c 100644 --- a/include/netft_driver/adapter_config.hpp +++ b/src/ros/adapter_config.hpp @@ -1,6 +1,6 @@ #pragma once -#include "netft_driver/types.hpp" +#include "netft/types.hpp" #include #include @@ -12,13 +12,17 @@ namespace netft_driver { struct AdapterParameters { std::string sensor_ip{"192.168.1.1"}; int sensor_port{49152}; + int http_port{80}; std::string frame_id{"netft_link"}; std::string wrench_topic{"/netft/wrench"}; std::string bias_service{"/netft/bias"}; + bool use_sensor_calibration{true}; double counts_per_force{1000000.0}; double counts_per_torque{1000000.0}; double publish_rate{0.0}; double receive_timeout{0.1}; + double configuration_connect_timeout{0.5}; + double configuration_timeout{1.0}; double reconnect_initial_delay{0.25}; double reconnect_max_delay{5.0}; double diagnostics_rate{1.0}; @@ -28,13 +32,13 @@ struct AdapterParameters { }; struct AdapterConfig { - ClientConfig client; + netft::Config client; std::string frame_id; std::string wrench_topic; std::string bias_service; - double diagnostics_rate; - double expected_rdt_rate; - double rate_tolerance; + double diagnostics_rate{}; + double expected_rdt_rate{}; + double rate_tolerance{}; }; inline void validate_adapter_config(const std::string & frame_id, const std::string & wrench_topic, @@ -57,24 +61,35 @@ inline AdapterConfig map_adapter_parameters(const AdapterParameters & parameters { AdapterConfig mapped; mapped.client.sensor_host = parameters.sensor_ip; - mapped.client.sensor_port = parameters.sensor_port; - mapped.frame_id = parameters.frame_id; - mapped.wrench_topic = parameters.wrench_topic; - mapped.bias_service = parameters.bias_service; - mapped.client.counts_per_force = parameters.counts_per_force; - mapped.client.counts_per_torque = parameters.counts_per_torque; - mapped.client.publish_rate = parameters.publish_rate; + mapped.client.rdt_port = parameters.sensor_port; + mapped.client.http_port = parameters.http_port; mapped.client.receive_timeout = std::chrono::duration{parameters.receive_timeout}; + mapped.client.configuration_connect_timeout = + std::chrono::duration{parameters.configuration_connect_timeout}; + mapped.client.configuration_timeout = + std::chrono::duration{parameters.configuration_timeout}; mapped.client.reconnect_initial_delay = std::chrono::duration{parameters.reconnect_initial_delay}; mapped.client.reconnect_max_delay = std::chrono::duration{parameters.reconnect_max_delay}; + mapped.client.sample_rate_limit_hz = parameters.publish_rate; + mapped.client.deliver_samples_with_error_status = parameters.publish_on_error; + if (!parameters.use_sensor_calibration) { + mapped.client.calibration_override = netft::Calibration{ + parameters.counts_per_force, + parameters.counts_per_torque, + netft::ForceUnit::Newton, + netft::TorqueUnit::NewtonMeter, + }; + } + mapped.frame_id = parameters.frame_id; + mapped.wrench_topic = parameters.wrench_topic; + mapped.bias_service = parameters.bias_service; mapped.diagnostics_rate = parameters.diagnostics_rate; mapped.expected_rdt_rate = parameters.expected_rdt_rate; mapped.rate_tolerance = parameters.rate_tolerance; - mapped.client.publish_on_error = parameters.publish_on_error; - validate(mapped.client); + netft::validate(mapped.client); validate_adapter_config(mapped.frame_id, mapped.wrench_topic, mapped.bias_service, mapped.diagnostics_rate); if (!std::isfinite(mapped.expected_rdt_rate) || mapped.expected_rdt_rate <= 0.0) { diff --git a/src/ros/diagnostics.cpp b/src/ros/diagnostics.cpp new file mode 100644 index 0000000..8f1867e --- /dev/null +++ b/src/ros/diagnostics.cpp @@ -0,0 +1,221 @@ +#include "ros/diagnostics.hpp" + +#include "netft/status.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace netft_driver { +namespace { + +std::string number(const double value, const int precision) +{ + std::ostringstream output; + output << std::fixed << std::setprecision(precision) << value; + return output.str(); +} + +std::string number(const std::uint64_t value) +{ + return std::to_string(value); +} + +std::string calibration_number(const double value) +{ + std::ostringstream output; + output << std::setprecision(std::numeric_limits::max_digits10) << value; + return output.str(); +} + +std::string sequence(const std::optional & value) +{ + return value ? std::to_string(*value) : "none"; +} + +std::string status_code(const std::uint32_t status) +{ + std::ostringstream output; + output << "0x" << std::hex << std::nouppercase << std::setw(8) << std::setfill('0') << status; + return output.str(); +} + +std::string_view configuration_source(const netft::CalibrationSource source) +{ + return source == netft::CalibrationSource::Sensor ? "sensor" : "override"; +} + +std::uint64_t delta(const std::uint64_t current, std::uint64_t & previous) +{ + const auto difference = current >= previous ? current - previous : 0; + previous = current; + return difference; +} + +} // namespace + +DiagnosticEvaluator::DiagnosticEvaluator(const double expected_rdt_rate, const double rate_tolerance) +: expected_rdt_rate_(expected_rdt_rate), rate_tolerance_(rate_tolerance) +{ + if (!std::isfinite(expected_rdt_rate) || expected_rdt_rate <= 0.0) { + throw std::invalid_argument{"expected_rdt_rate must be finite and greater than zero"}; + } + if (!std::isfinite(rate_tolerance) || rate_tolerance < 0.0 || rate_tolerance > 1.0) { + throw std::invalid_argument{"rate_tolerance must be between 0 and 1"}; + } +} + +DiagnosticReport DiagnosticEvaluator::evaluate(const netft::HealthSnapshot & snapshot) +{ + std::vector> values{ + {"state", std::string{netft::to_string(snapshot.state)}}, + {"sensor", snapshot.sensor_host + ":" + std::to_string(snapshot.rdt_port)}, + {"last_rdt_sequence", sequence(snapshot.last_rdt_sequence)}, + {"last_ft_sequence", sequence(snapshot.last_ft_sequence)}, + {"last_ft_progress", snapshot.last_ft_progress}, + {"device_status", status_code(snapshot.last_status)}, + {"active_status", netft::decode_status(snapshot.last_status)}, + {"receive_rate_hz", number(snapshot.receive_rate_hz, 1)}, + {"expected_receive_rate_hz", number(expected_rdt_rate_, 1)}, + {"rate_tolerance", number(rate_tolerance_, 3)}, + {"delivery_rate_hz", number(snapshot.delivery_rate_hz, 1)}, + {"received_count", number(snapshot.received_count)}, + {"delivered_count", number(snapshot.delivered_count)}, + {"rate_limited_count", number(snapshot.rate_limited_count)}, + {"device_error_count", number(snapshot.device_error_count)}, + {"lost_count", number(snapshot.lost_count)}, + {"duplicate_count", number(snapshot.duplicate_count)}, + {"out_of_order_count", number(snapshot.out_of_order_count)}, + {"ft_stall_count", number(snapshot.ft_stall_count)}, + {"ft_backward_count", number(snapshot.ft_backward_count)}, + {"ft_restart_count", number(snapshot.ft_restart_count)}, + {"malformed_count", number(snapshot.malformed_count)}, + {"malformed_storm_threshold", number(kMalformedStormThreshold)}, + {"malformed_storm_window", "between_diagnostic_updates"}, + {"reconnect_count", number(snapshot.reconnect_count)}, + {"timeout_count", number(snapshot.timeout_count)}, + {"callback_error_count", number(snapshot.callback_error_count)}, + {"last_record_age_s", + snapshot.last_record_age ? number(snapshot.last_record_age->count(), 3) : "none"}, + {"last_error", snapshot.last_error}, + }; + + if (snapshot.sensor_configuration) { + const auto & configuration = *snapshot.sensor_configuration; + values.emplace_back("configuration_source", + std::string{configuration_source(configuration.source)}); + values.emplace_back("force_unit", + std::string{netft::to_string(configuration.calibration.force_unit)}); + values.emplace_back("torque_unit", + std::string{netft::to_string(configuration.calibration.torque_unit)}); + values.emplace_back("configuration_revision", number(configuration.revision)); + values.emplace_back("calibration_change_count", number(snapshot.calibration_change_count)); + values.emplace_back("counts_per_force_unit", + calibration_number(configuration.calibration.counts_per_force_unit)); + values.emplace_back("counts_per_torque_unit", + calibration_number(configuration.calibration.counts_per_torque_unit)); + } + + const auto report = [&](const int level, std::string message, const char * log_key) { + return DiagnosticReport{level, std::move(message), values, log_key}; + }; + + if (snapshot.state == netft::ClientState::Faulted || + snapshot.fault_code != netft::FaultCode::None) + { + const auto detail = snapshot.last_error.empty() ? + std::string{netft::to_string(snapshot.fault_code)} : snapshot.last_error; + return report(2, "faulted: " + detail, "faulted"); + } + + const auto lost = delta(snapshot.lost_count, lost_); + const auto device_error = delta(snapshot.device_error_count, device_error_); + const auto timeout = delta(snapshot.timeout_count, timeout_); + const auto malformed = delta(snapshot.malformed_count, malformed_); + const auto ft_stall = delta(snapshot.ft_stall_count, ft_stall_); + const auto ft_backward = delta(snapshot.ft_backward_count, ft_backward_); + const auto ft_restart = delta(snapshot.ft_restart_count, ft_restart_); + const auto callback_error = delta(snapshot.callback_error_count, callback_error_); + + if (snapshot.state == netft::ClientState::Backoff) { + return report(2, "connection lost; reconnecting", "backoff"); + } + if (snapshot.state == netft::ClientState::Stopped) { + return report(2, "client stopped", "stopped"); + } + + const auto status_severity = netft::classify_status(snapshot.last_status); + if (status_severity == netft::StatusSeverity::Error) { + return report(2, "device error: " + netft::decode_status(snapshot.last_status), "device_error"); + } + if (device_error != 0) { + return report(2, "serious device status observed since last diagnostic", "device_error_event"); + } + if (timeout != 0) { + return report(2, "receive timeout observed since last diagnostic", "receive_timeout"); + } + if (malformed >= kMalformedStormThreshold) { + return report(2, "malformed-packet storm observed since last diagnostic", "malformed_storm"); + } + if (ft_stall != 0) { + return report(2, "FT sequence stalled since last diagnostic", "ft_stall"); + } + if (ft_backward != 0) { + return report(2, "FT sequence moved backward since last diagnostic", "ft_backward"); + } + if (snapshot.state == netft::ClientState::Connecting) { + return report(1, "waiting for first RDT record", "connecting"); + } + if (status_severity == netft::StatusSeverity::Warn) { + return report(1, "monitor condition latched", "condition_latch"); + } + if (ft_restart != 0) { + return report(1, "FT device counter restarted since last diagnostic", "ft_restart"); + } + if (lost != 0) { + return report(1, "RDT records lost since last diagnostic", "packet_loss"); + } + + const auto lower_rate = expected_rdt_rate_ * (1.0 - rate_tolerance_); + const auto upper_rate = expected_rdt_rate_ * (1.0 + rate_tolerance_); + if (snapshot.receive_rate_hz < lower_rate || snapshot.receive_rate_hz > upper_rate) { + return report(1, "receive rate outside configured tolerance", "receive_rate"); + } + if (callback_error != 0) { + return report(1, "sample callback failed since last diagnostic", "callback_error"); + } + return report(0, "streaming normally", "healthy"); +} + +FaultLogThrottle::FaultLogThrottle(const double repeat_interval) +: repeat_interval_(repeat_interval) +{ + if (!std::isfinite(repeat_interval) || repeat_interval <= 0.0) { + throw std::invalid_argument{"repeat_interval must be finite and greater than zero"}; + } +} + +bool FaultLogThrottle::should_log(const DiagnosticReport & report, const double now) +{ + if (report.level <= 0) { + active_ = false; + return false; + } + + if (!active_ || report.level != level_ || report.log_key != key_ || + now - last_log_ >= repeat_interval_) + { + active_ = true; + level_ = report.level; + key_ = report.log_key; + last_log_ = now; + return true; + } + return false; +} + +} // namespace netft_driver diff --git a/src/ros/diagnostics.hpp b/src/ros/diagnostics.hpp new file mode 100644 index 0000000..6dff145 --- /dev/null +++ b/src/ros/diagnostics.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include "netft/types.hpp" + +#include +#include +#include +#include +#include + +namespace netft_driver { + +inline constexpr std::uint64_t kMalformedStormThreshold = 10; +inline constexpr std::array kDiagnosticValueKeys{ + "state", "sensor", "last_rdt_sequence", "last_ft_sequence", "last_ft_progress", + "device_status", "active_status", "receive_rate_hz", "expected_receive_rate_hz", + "rate_tolerance", "delivery_rate_hz", "received_count", "delivered_count", + "rate_limited_count", "device_error_count", "lost_count", "duplicate_count", + "out_of_order_count", "ft_stall_count", "ft_backward_count", "ft_restart_count", + "malformed_count", "malformed_storm_threshold", "malformed_storm_window", + "reconnect_count", "timeout_count", "callback_error_count", "last_record_age_s", + "last_error", "configuration_source", "force_unit", "torque_unit", + "configuration_revision", "calibration_change_count", "counts_per_force_unit", + "counts_per_torque_unit"}; + +struct DiagnosticReport { + int level{}; + std::string message; + std::vector> values; + std::string log_key; +}; + +class DiagnosticEvaluator { +public: + DiagnosticEvaluator(double expected_rdt_rate, double rate_tolerance); + DiagnosticEvaluator(double expected_rdt_rate, bool rate_tolerance) = delete; + + DiagnosticReport evaluate(const netft::HealthSnapshot & snapshot); + +private: + double expected_rdt_rate_; + double rate_tolerance_; + std::uint64_t lost_{}; + std::uint64_t device_error_{}; + std::uint64_t timeout_{}; + std::uint64_t malformed_{}; + std::uint64_t ft_stall_{}; + std::uint64_t ft_backward_{}; + std::uint64_t ft_restart_{}; + std::uint64_t callback_error_{}; +}; + +class FaultLogThrottle { +public: + explicit FaultLogThrottle(double repeat_interval = 10.0); + + bool should_log(const DiagnosticReport & report, double now); + +private: + double repeat_interval_; + bool active_{false}; + int level_{}; + std::string key_; + double last_log_{}; +}; + +} // namespace netft_driver diff --git a/include/netft_driver/ros2_control_compat.hpp b/src/ros/ros2_control_compat.hpp similarity index 65% rename from include/netft_driver/ros2_control_compat.hpp rename to src/ros/ros2_control_compat.hpp index f2d1a62..5ee6a50 100644 --- a/include/netft_driver/ros2_control_compat.hpp +++ b/src/ros/ros2_control_compat.hpp @@ -1,12 +1,8 @@ #pragma once -#include "netft_driver/types.hpp" - #include #include -#include - #ifdef NETFT_ROS2_CONTROL_MODERN_API #include #endif @@ -33,15 +29,4 @@ inline const hardware_interface::HardwareInfo & hardware_info( #error "A ros2_control API family must be selected by CMake" #endif -#ifdef NETFT_ROS2_CONTROL_TESTING -void test_force_initial_sample_once() noexcept; -void test_fail_state_write_once_at(std::size_t axis) noexcept; -void test_break_active_socket() noexcept; -bool test_read_active_instance() noexcept; -FaultCode test_active_fault_code() noexcept; -bool test_interface_write_fault_latched() noexcept; -void test_throw_executor_cancel_once() noexcept; -int test_auxiliary_thread_count() noexcept; -#endif - } // namespace netft_driver::ros2_control_compat diff --git a/src/ros/ros2_control_test_access.hpp b/src/ros/ros2_control_test_access.hpp new file mode 100644 index 0000000..6af2953 --- /dev/null +++ b/src/ros/ros2_control_test_access.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +namespace netft_driver::ros2_control_test_access { + +enum class FaultCode { + None, + SensorConfiguration, + Timeout, + Socket, + SeriousStatus, + FtStall, + FtBackward, + MalformedStorm, + Callback, +}; + +struct ActivityCounters { + std::uint64_t sample_generation; +}; + +void test_force_initial_sample_once() noexcept; +void test_fail_state_write_once_at(std::size_t axis) noexcept; +void test_latch_active_fault(FaultCode fault) noexcept; +bool test_read_active_instance() noexcept; +FaultCode test_active_fault_code() noexcept; +FaultCode test_active_client_fault_code() noexcept; +FaultCode test_active_latched_fault_code() noexcept; +ActivityCounters test_active_activity_counters() noexcept; +bool test_interface_write_fault_latched() noexcept; +void test_throw_executor_cancel_once() noexcept; +int test_auxiliary_thread_count() noexcept; + +} // namespace netft_driver::ros2_control_test_access diff --git a/src/ros/unit_conversion.cpp b/src/ros/unit_conversion.cpp new file mode 100644 index 0000000..7cd2291 --- /dev/null +++ b/src/ros/unit_conversion.cpp @@ -0,0 +1,69 @@ +#include "ros/unit_conversion.hpp" + +#include + +namespace netft_driver { + +double force_scale_to_newtons(netft::ForceUnit unit) +{ + switch (unit) { + case netft::ForceUnit::Newton: + return 1.0; + case netft::ForceUnit::KiloNewton: + return 1000.0; + case netft::ForceUnit::PoundForce: + return 4.4482216152605; + case netft::ForceUnit::KiloPoundForce: + return 4448.2216152605; + case netft::ForceUnit::KilogramForce: + return 9.80665; + case netft::ForceUnit::Unknown: + throw std::invalid_argument{"Unknown force unit"}; + } + + throw std::invalid_argument{"Unknown force unit"}; +} + +double torque_scale_to_newton_metres(netft::TorqueUnit unit) +{ + switch (unit) { + case netft::TorqueUnit::NewtonMeter: + return 1.0; + case netft::TorqueUnit::NewtonMillimeter: + return 0.001; + case netft::TorqueUnit::PoundForceInch: + return 0.1129848290276167; + case netft::TorqueUnit::PoundForceFoot: + return 1.3558179483314004; + case netft::TorqueUnit::KilogramForceCentimeter: + return 0.0980665; + case netft::TorqueUnit::KiloNewtonMeter: + return 1000.0; + case netft::TorqueUnit::Unknown: + throw std::invalid_argument{"Unknown torque unit"}; + } + + throw std::invalid_argument{"Unknown torque unit"}; +} + +SiSample to_si_sample(const netft::Sample & sample) +{ + const auto force_scale = force_scale_to_newtons(sample.force_unit); + const auto torque_scale = torque_scale_to_newton_metres(sample.torque_unit); + + SiSample si; + si.rdt_sequence = sample.rdt_sequence; + si.ft_sequence = sample.ft_sequence; + si.status = sample.status; + si.configuration_revision = sample.configuration_revision; + si.received_at = sample.received_at; + + for (std::size_t index = 0; index < si.force.size(); ++index) { + si.force[index] = sample.force[index] * force_scale; + si.torque[index] = sample.torque[index] * torque_scale; + } + + return si; +} + +} // namespace netft_driver diff --git a/src/ros/unit_conversion.hpp b/src/ros/unit_conversion.hpp new file mode 100644 index 0000000..2322de8 --- /dev/null +++ b/src/ros/unit_conversion.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include "netft/types.hpp" + +#include +#include +#include + +namespace netft_driver { + +struct SiSample { + std::uint32_t rdt_sequence{}; + std::uint32_t ft_sequence{}; + std::uint32_t status{}; + std::array force{}; + std::array torque{}; + std::uint64_t configuration_revision{}; + std::chrono::steady_clock::time_point received_at{}; +}; + +double force_scale_to_newtons(netft::ForceUnit unit); +double torque_scale_to_newton_metres(netft::TorqueUnit unit); +SiSample to_si_sample(const netft::Sample & sample); + +} // namespace netft_driver diff --git a/src/ros1_node.cpp b/src/ros1_node.cpp index b4b3f88..8563b5f 100644 --- a/src/ros1_node.cpp +++ b/src/ros1_node.cpp @@ -5,11 +5,11 @@ #include #include -#include "netft_driver/client.hpp" -#include "netft_driver/adapter_config.hpp" -#include "netft_driver/status.hpp" +#include "netft/client.hpp" +#include "ros/adapter_config.hpp" +#include "ros/diagnostics.hpp" +#include "ros/unit_conversion.hpp" -#include #include #include @@ -32,9 +32,9 @@ class NetFTRos1Node final { diagnostic_timer_ = public_node_.createTimer(ros::Duration{1.0 / diagnostics_rate_}, &NetFTRos1Node::publish_diagnostics, this); } - void start() { client_.start([this](const netft_driver::WrenchSample & sample) { publish_wrench(sample); }); } + void start() { client_.start([this](const netft::Sample & sample) { publish_wrench(sample); }); } void stop() noexcept { client_.stop(); } - const netft_driver::ClientConfig & config() const { return config_; } + const netft::Config & config() const { return config_; } const std::string & frame_id() const { return frame_id_; } const std::string & wrench_topic() const { return wrench_topic_; } const std::string & bias_service() const { return bias_service_name_; } @@ -43,19 +43,28 @@ class NetFTRos1Node final { double rate_tolerance() const { return rate_tolerance_; } private: - netft_driver::ClientConfig read_config() + netft::Config read_config() { netft_driver::AdapterParameters parameters; parameter(private_node_, "sensor_ip", parameters.sensor_ip, parameters.sensor_ip); parameter(private_node_, "sensor_port", parameters.sensor_port, parameters.sensor_port); + parameter(private_node_, "http_port", parameters.http_port, parameters.http_port); parameter(private_node_, "frame_id", parameters.frame_id, parameters.frame_id); parameter(private_node_, "wrench_topic", parameters.wrench_topic, parameters.wrench_topic); parameter(private_node_, "bias_service", parameters.bias_service, parameters.bias_service); + parameter(private_node_, "use_sensor_calibration", parameters.use_sensor_calibration, + parameters.use_sensor_calibration); parameter(private_node_, "counts_per_force", parameters.counts_per_force, parameters.counts_per_force); parameter(private_node_, "counts_per_torque", parameters.counts_per_torque, parameters.counts_per_torque); parameter(private_node_, "publish_rate", parameters.publish_rate, parameters.publish_rate); parameter(private_node_, "receive_timeout", parameters.receive_timeout, parameters.receive_timeout); - parameter(private_node_, "reconnect_initial_delay", parameters.reconnect_initial_delay, parameters.reconnect_initial_delay); + parameter(private_node_, "configuration_connect_timeout", + parameters.configuration_connect_timeout, + parameters.configuration_connect_timeout); + parameter(private_node_, "configuration_timeout", parameters.configuration_timeout, + parameters.configuration_timeout); + parameter(private_node_, "reconnect_initial_delay", parameters.reconnect_initial_delay, + parameters.reconnect_initial_delay); parameter(private_node_, "reconnect_max_delay", parameters.reconnect_max_delay, parameters.reconnect_max_delay); parameter(private_node_, "diagnostics_rate", parameters.diagnostics_rate, parameters.diagnostics_rate); parameter(private_node_, "expected_rdt_rate", parameters.expected_rdt_rate, parameters.expected_rdt_rate); @@ -72,13 +81,14 @@ class NetFTRos1Node final { return config_; } - void publish_wrench(const netft_driver::WrenchSample & sample) + void publish_wrench(const netft::Sample & sample) { + const auto si = netft_driver::to_si_sample(sample); geometry_msgs::WrenchStamped message; message.header.stamp = ros::Time::now(); message.header.frame_id = frame_id_; - message.wrench.force.x = sample.force[0]; message.wrench.force.y = sample.force[1]; message.wrench.force.z = sample.force[2]; - message.wrench.torque.x = sample.torque[0]; message.wrench.torque.y = sample.torque[1]; message.wrench.torque.z = sample.torque[2]; + message.wrench.force.x = si.force[0]; message.wrench.force.y = si.force[1]; message.wrench.force.z = si.force[2]; + message.wrench.torque.x = si.torque[0]; message.wrench.torque.y = si.torque[1]; message.wrench.torque.z = si.torque[2]; wrench_publisher_.publish(message); } @@ -91,13 +101,13 @@ class NetFTRos1Node final { void publish_diagnostics(const ros::TimerEvent &) { - const auto snapshot = client_.health_snapshot(); + const auto snapshot = client_.health(); const auto report = evaluator_.evaluate(snapshot); diagnostic_msgs::DiagnosticStatus status; status.level = static_cast(report.level); status.name = "netft_driver: connection"; status.message = report.message; - status.hardware_id = snapshot.sensor_host + ":" + std::to_string(snapshot.sensor_port); + status.hardware_id = snapshot.sensor_host + ":" + std::to_string(snapshot.rdt_port); for (const auto & value : report.values) { diagnostic_msgs::KeyValue key_value; key_value.key = value.first; @@ -111,10 +121,10 @@ class NetFTRos1Node final { } ros::NodeHandle public_node_, private_node_; - netft_driver::ClientConfig config_; + netft::Config config_; std::string frame_id_, wrench_topic_, bias_service_name_; double diagnostics_rate_{1.0}, expected_rdt_rate_{2000.0}, rate_tolerance_{0.2}; - netft_driver::NetFTClient client_; + netft::Client client_; netft_driver::DiagnosticEvaluator evaluator_{2000.0, 0.2}; ros::Publisher wrench_publisher_, diagnostics_publisher_; ros::ServiceServer bias_service_; diff --git a/src/ros2_node.cpp b/src/ros2_node.cpp index 82202b1..165d019 100644 --- a/src/ros2_node.cpp +++ b/src/ros2_node.cpp @@ -6,9 +6,10 @@ #include #include -#include "netft_driver/client.hpp" -#include "netft_driver/adapter_config.hpp" -#include "netft_driver/status.hpp" +#include "netft/client.hpp" +#include "ros/adapter_config.hpp" +#include "ros/diagnostics.hpp" +#include "ros/unit_conversion.hpp" #include #include @@ -26,9 +27,9 @@ class NetFTRos2Node final : public rclcpp::Node { diagnostic_timer_ = rclcpp::create_timer(this, get_clock(), std::chrono::duration{1.0 / diagnostics_rate_}, [this] { publish_diagnostics(); }); } - void start() { client_.start([this](const netft_driver::WrenchSample & sample) { publish_wrench(sample); }); } + void start() { client_.start([this](const netft::Sample & sample) { publish_wrench(sample); }); } void stop() noexcept { client_.stop(); } - const netft_driver::ClientConfig & config() const { return config_; } + const netft::Config & config() const { return config_; } const std::string & frame_id() const { return frame_id_; } const std::string & wrench_topic() const { return wrench_topic_; } const std::string & bias_service() const { return bias_service_name_; } @@ -43,19 +44,27 @@ class NetFTRos2Node final : public rclcpp::Node { return declare_parameter(name, fallback); } - netft_driver::ClientConfig read_config() + netft::Config read_config() { netft_driver::AdapterParameters parameters; parameters.sensor_ip = parameter("sensor_ip", parameters.sensor_ip); parameters.sensor_port = parameter("sensor_port", parameters.sensor_port); + parameters.http_port = parameter("http_port", parameters.http_port); parameters.frame_id = parameter("frame_id", parameters.frame_id); parameters.wrench_topic = parameter("wrench_topic", parameters.wrench_topic); parameters.bias_service = parameter("bias_service", parameters.bias_service); + parameters.use_sensor_calibration = + parameter("use_sensor_calibration", parameters.use_sensor_calibration); parameters.counts_per_force = parameter("counts_per_force", parameters.counts_per_force); parameters.counts_per_torque = parameter("counts_per_torque", parameters.counts_per_torque); parameters.publish_rate = parameter("publish_rate", parameters.publish_rate); parameters.receive_timeout = parameter("receive_timeout", parameters.receive_timeout); - parameters.reconnect_initial_delay = parameter("reconnect_initial_delay", parameters.reconnect_initial_delay); + parameters.configuration_connect_timeout = parameter( + "configuration_connect_timeout", parameters.configuration_connect_timeout); + parameters.configuration_timeout = + parameter("configuration_timeout", parameters.configuration_timeout); + parameters.reconnect_initial_delay = + parameter("reconnect_initial_delay", parameters.reconnect_initial_delay); parameters.reconnect_max_delay = parameter("reconnect_max_delay", parameters.reconnect_max_delay); parameters.diagnostics_rate = parameter("diagnostics_rate", parameters.diagnostics_rate); parameters.expected_rdt_rate = parameter("expected_rdt_rate", parameters.expected_rdt_rate); @@ -72,13 +81,14 @@ class NetFTRos2Node final : public rclcpp::Node { return config_; } - void publish_wrench(const netft_driver::WrenchSample & sample) + void publish_wrench(const netft::Sample & sample) { + const auto si = netft_driver::to_si_sample(sample); geometry_msgs::msg::WrenchStamped message; message.header.stamp = get_clock()->now(); message.header.frame_id = frame_id_; - message.wrench.force.x = sample.force[0]; message.wrench.force.y = sample.force[1]; message.wrench.force.z = sample.force[2]; - message.wrench.torque.x = sample.torque[0]; message.wrench.torque.y = sample.torque[1]; message.wrench.torque.z = sample.torque[2]; + message.wrench.force.x = si.force[0]; message.wrench.force.y = si.force[1]; message.wrench.force.z = si.force[2]; + message.wrench.torque.x = si.torque[0]; message.wrench.torque.y = si.torque[1]; message.wrench.torque.z = si.torque[2]; wrench_publisher_->publish(message); } @@ -90,13 +100,13 @@ class NetFTRos2Node final : public rclcpp::Node { void publish_diagnostics() { - const auto snapshot = client_.health_snapshot(); + const auto snapshot = client_.health(); const auto report = evaluator_.evaluate(snapshot); diagnostic_msgs::msg::DiagnosticStatus status; status.level = static_cast(report.level); status.name = "netft_driver: connection"; status.message = report.message; - status.hardware_id = snapshot.sensor_host + ":" + std::to_string(snapshot.sensor_port); + status.hardware_id = snapshot.sensor_host + ":" + std::to_string(snapshot.rdt_port); for (const auto & value : report.values) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = value.first; @@ -109,10 +119,10 @@ class NetFTRos2Node final : public rclcpp::Node { diagnostics_publisher_->publish(array); } - netft_driver::ClientConfig config_; + netft::Config config_; std::string frame_id_, wrench_topic_, bias_service_name_; double diagnostics_rate_{1.0}, expected_rdt_rate_{2000.0}, rate_tolerance_{0.2}; - netft_driver::NetFTClient client_; + netft::Client client_; netft_driver::DiagnosticEvaluator evaluator_{2000.0, 0.2}; rclcpp::Publisher::SharedPtr wrench_publisher_; rclcpp::Publisher::SharedPtr diagnostics_publisher_; diff --git a/src/status.cpp b/src/status.cpp deleted file mode 100644 index ca61810..0000000 --- a/src/status.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "netft_driver/status.hpp" - -#include -#include -#include -#include -#include - -namespace netft_driver { -namespace { -constexpr std::uint32_t kRestartLowMax = 0x0000ffffU; -constexpr std::uint32_t kRestartMinBaseline = kRestartLowMax + 1U; -const std::pair kStatusBits[] = { - {0x80000000U, "error summary"}, {0x40000000U, "CPU or RAM error"}, {0x20000000U, "digital board error"}, {0x10000000U, "analog board error"}, {0x08000000U, "serial link communication error"}, {0x04000000U, "program memory verification error"}, {0x02000000U, "halted due to configuration errors"}, {0x01000000U, "settings validation error"}, {0x00800000U, "configuration incompatible with calibration"}, {0x00400000U, "network communication failure"}, {0x00200000U, "CAN communication error"}, {0x00100000U, "RDT communication error"}, {0x00080000U, "EtherNet/IP protocol failure"}, {0x00040000U, "DeviceNet protocol failure"}, {0x00020000U, "transducer saturation or A/D error"}, {0x00010000U, "monitor condition latched"}, {0x00004000U, "watchdog timeout error"}, {0x00002000U, "stack check error"}, {0x00001000U, "serial EEPROM I2C failure"}, {0x00000800U, "serial flash SPI failure"}, {0x00000400U, "analog board watchdog timeout"}, {0x00000200U, "excessive strain gage excitation current"}, {0x00000100U, "insufficient strain gage excitation current"}, {0x00000080U, "artificial analog ground out of range"}, {0x00000040U, "analog board power supply too high"}, {0x00000020U, "analog board power supply too low"}, {0x00000010U, "serial link data unavailable"}, {0x00000008U, "reference voltage or power monitoring error"}, {0x00000004U, "internal temperature error"}, {0x00000002U, "HTTP protocol failure"}}; -std::string state_name(ClientState state) { switch (state) { case ClientState::Stopped: return "stopped"; case ClientState::Connecting: return "connecting"; case ClientState::Streaming: return "streaming"; case ClientState::Backoff: return "backoff"; case ClientState::Faulted: return "faulted"; } return "unknown"; } -std::string number(double value, int precision) { std::ostringstream out; out << std::fixed << std::setprecision(precision) << value; return out.str(); } -std::string number(std::uint64_t value) { return std::to_string(value); } -std::string sequence(const std::optional & value) { return value ? std::to_string(*value) : "none"; } -std::string fault_name(FaultCode fault) { switch (fault) { case FaultCode::None: return "unknown fault"; case FaultCode::Timeout: return "receive timeout"; case FaultCode::Socket: return "socket failure"; case FaultCode::SeriousStatus: return "serious device status"; case FaultCode::FtStall: return "FT sequence stall"; case FaultCode::FtBackward: return "FT sequence backward"; case FaultCode::MalformedStorm: return "malformed-packet storm"; case FaultCode::Callback: return "sample callback failure"; } return "unknown fault"; } -} - -DiagnosticSeverity classify_status(const std::uint32_t status) { return status == 0 ? DiagnosticSeverity::Ok : status == 0x80010000U ? DiagnosticSeverity::Warn : DiagnosticSeverity::Error; } -std::string decode_status(const std::uint32_t status) { if (status == 0) return "healthy"; std::string result; for (const auto & [mask, name] : kStatusBits) if ((status & mask) != 0) { if (!result.empty()) result += ", "; result += name; } return result; } -SequenceObservation RdtSequenceTracker::observe(const std::uint32_t current) { if (!has_last_) { has_last_ = true; previous_ = current; return {SequenceKind::First, 0}; } const std::uint32_t delta = current - previous_; if (delta == 0) return {SequenceKind::Duplicate, 0}; if (delta < 0x80000000U) { previous_ = current; return delta == 1 ? SequenceObservation{SequenceKind::Contiguous, 0} : SequenceObservation{SequenceKind::Gap, delta - 1}; } return {SequenceKind::OutOfOrder, 0}; } -void RdtSequenceTracker::reset() { has_last_ = false; } -void FtSequenceTracker::begin_session() { has_restart_candidate_ = false; } -FtSequenceObservation FtSequenceTracker::observe(const std::uint32_t current) { if (!has_last_) { has_last_ = true; previous_ = current; return {FtSequenceKind::First}; } if (current == previous_) { has_restart_candidate_ = false; return {FtSequenceKind::Stall}; } const std::uint32_t delta = current - previous_; if (delta < 0x80000000U) { previous_ = current; has_restart_candidate_ = false; return {FtSequenceKind::Forward}; } if (previous_ >= kRestartMinBaseline && has_restart_candidate_ && restart_candidate_ <= kRestartLowMax && current <= kRestartLowMax) { const std::uint32_t candidate_delta = current - restart_candidate_; if (candidate_delta > 0 && candidate_delta < 0x80000000U) { previous_ = current; has_restart_candidate_ = false; return {FtSequenceKind::Restart}; } } restart_candidate_ = current; has_restart_candidate_ = true; return {FtSequenceKind::Backward}; } -DiagnosticEvaluator::DiagnosticEvaluator(double expected, double tolerance) : expected_rdt_rate_(expected), rate_tolerance_(tolerance) { if (!std::isfinite(expected) || expected <= 0) throw std::invalid_argument{"expected_rdt_rate must be finite and greater than zero"}; if (!std::isfinite(tolerance) || tolerance < 0 || tolerance > 1) throw std::invalid_argument{"rate_tolerance must be between 0 and 1"}; } -DiagnosticReport DiagnosticEvaluator::evaluate(const HealthSnapshot & s) { - std::vector> values{{"state", state_name(s.state)}, {"sensor", s.sensor_host + ":" + std::to_string(s.sensor_port)}, {"last_rdt_sequence", sequence(s.last_rdt_sequence)}, {"last_ft_sequence", sequence(s.last_ft_sequence)}, {"last_ft_progress", s.last_ft_progress}, {"device_status", "0x" + [&] { std::ostringstream out; out << std::hex << std::nouppercase << std::setw(8) << std::setfill('0') << s.last_status; return out.str(); }()}, {"active_status", decode_status(s.last_status)}, {"receive_rate_hz", number(s.receive_rate, 1)}, {"expected_receive_rate_hz", number(expected_rdt_rate_, 1)}, {"rate_tolerance", number(rate_tolerance_, 3)}, {"publish_rate_hz", number(s.publish_rate, 1)}, {"received_count", number(s.received_count)}, {"published_count", number(s.published_count)}, {"rate_dropped_count", number(s.rate_dropped_count)}, {"device_error_count", number(s.device_error_count)}, {"lost_count", number(s.lost_count)}, {"duplicate_count", number(s.duplicate_count)}, {"out_of_order_count", number(s.out_of_order_count)}, {"ft_stall_count", number(s.ft_stall_count)}, {"ft_backward_count", number(s.ft_backward_count)}, {"ft_restart_count", number(s.ft_restart_count)}, {"malformed_count", number(s.malformed_count)}, {"malformed_storm_threshold", number(kMalformedStormThreshold)}, {"malformed_storm_window", "between_diagnostic_updates"}, {"reconnect_count", number(s.reconnect_count)}, {"timeout_count", number(s.timeout_count)}, {"callback_error_count", number(s.callback_error_count)}, {"last_record_age_s", s.last_record_age ? number(s.last_record_age->count(), 3) : "none"}, {"last_error", s.last_error}}; - const auto report = [&](int level, const std::string & message, const char * key) { return DiagnosticReport{level, message, values, key}; }; - if (s.state == ClientState::Faulted || s.fault_code != FaultCode::None) return report(2, "faulted: " + (s.last_error.empty() ? fault_name(s.fault_code) : s.last_error), "faulted"); - const auto delta = [](std::uint64_t now, std::uint64_t & previous) { const auto value = now >= previous ? now - previous : 0; previous = now; return value; }; - const auto lost = delta(s.lost_count, lost_), device = delta(s.device_error_count, device_error_), timeout = delta(s.timeout_count, timeout_), malformed = delta(s.malformed_count, malformed_), stall = delta(s.ft_stall_count, ft_stall_), backward = delta(s.ft_backward_count, ft_backward_), restart = delta(s.ft_restart_count, ft_restart_); - if (s.state == ClientState::Backoff) return report(2, "connection lost; reconnecting", "backoff"); if (s.state == ClientState::Stopped) return report(2, "client stopped", "stopped"); const auto severity = classify_status(s.last_status); if (severity == DiagnosticSeverity::Error) return report(2, "device error: " + decode_status(s.last_status), "device_error"); if (device) return report(2, "serious device status observed since last diagnostic", "device_error_event"); if (timeout) return report(2, "receive timeout observed since last diagnostic", "receive_timeout"); if (malformed >= kMalformedStormThreshold) return report(2, "malformed-packet storm: " + std::to_string(malformed) + " packets since last diagnostic", "malformed_storm"); if (stall) return report(2, "FT sequence stalled " + std::to_string(stall) + " times since last diagnostic", "ft_stall"); if (backward) return report(2, "FT sequence moved backward " + std::to_string(backward) + " times since last diagnostic", "ft_backward"); if (s.state == ClientState::Connecting) return report(1, "waiting for first RDT record", "connecting"); if (severity == DiagnosticSeverity::Warn) return report(1, "monitor condition latched", "condition_latch"); if (restart) return report(1, "FT device counter restarted since last diagnostic", "ft_restart"); if (lost) return report(1, std::to_string(lost) + " RDT records lost", "packet_loss"); const double lower = expected_rdt_rate_ * (1 - rate_tolerance_), upper = expected_rdt_rate_ * (1 + rate_tolerance_); if (s.receive_rate < lower || s.receive_rate > upper) return report(1, "receive rate " + number(s.receive_rate, 1) + " Hz outside " + number(lower, 1) + "-" + number(upper, 1) + " Hz", "receive_rate"); if (s.callback_error_count) return report(1, "sample callback has failed", "callback_error"); return report(0, "streaming normally", "healthy"); -} -FaultLogThrottle::FaultLogThrottle(double repeat_interval) : repeat_interval_(repeat_interval) { if (!std::isfinite(repeat_interval) || repeat_interval <= 0) throw std::invalid_argument{"repeat_interval must be finite and greater than zero"}; } -bool FaultLogThrottle::should_log(const DiagnosticReport & report, double now) { if (report.level <= 0) { active_ = false; return false; } const std::string key = report.log_key.empty() ? report.message : report.log_key; if (!active_ || report.level != level_ || key != key_ || now - last_log_ >= repeat_interval_) { active_ = true; level_ = report.level; key_ = key; last_log_ = now; return true; } return false; } -} // namespace netft_driver diff --git a/test/core/CMakeLists.txt b/test/core/CMakeLists.txt new file mode 100644 index 0000000..771a3e1 --- /dev/null +++ b/test/core/CMakeLists.txt @@ -0,0 +1,77 @@ +if(TARGET GTest::gtest_main) + set(NETFT_GTEST_MAIN_TARGET GTest::gtest_main) +elseif(TARGET GTest::Main) + set(NETFT_GTEST_MAIN_TARGET GTest::Main) +else() + message(FATAL_ERROR "GoogleTest does not provide a main target") +endif() + +function(add_netft_snapshot_test target source) + add_executable(${target} ${source} ${ARGN}) + target_include_directories(${target} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} + ${NETFT_SNAPSHOT_SOURCE_DIR}/src + ) + target_link_libraries(${target} PRIVATE + netft_core ${NETFT_GTEST_MAIN_TARGET}) + add_test(NAME ${target} COMMAND ${target}) +endfunction() + +add_netft_snapshot_test(netft_snapshot_types test_types.cpp) +add_netft_snapshot_test(netft_snapshot_status test_status.cpp) + +add_netft_snapshot_test(netft_snapshot_protocol + test_protocol.cpp + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/protocol.cpp) + +add_netft_snapshot_test(netft_snapshot_sequence + test_sequence.cpp + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/sequence.cpp) + +add_netft_snapshot_test(netft_snapshot_fault_latch + test_fault_latch.cpp + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/fault_latch.cpp) + +add_netft_snapshot_test(netft_snapshot_discovery + test_discovery.cpp + support/fake_http_server.cpp + ${NETFT_XML_CONFIG_SOURCE}) + +add_netft_snapshot_test(netft_compat_discovery + test_discovery.cpp + support/fake_http_server.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../../src/compat/xml_config.cpp) + +add_netft_snapshot_test(netft_snapshot_client_stream + test_client_stream.cpp + support/fake_http_server.cpp + support/fake_sensor.cpp) + +add_netft_snapshot_test(netft_snapshot_client_recovery + test_client_recovery.cpp + support/fake_http_server.cpp + support/fake_sensor.cpp) + +add_library(netft_snapshot_client_impl_test STATIC + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/client_impl.cpp + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/fault_latch.cpp + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/posix_transport.cpp + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/protocol.cpp + ${NETFT_SNAPSHOT_SOURCE_DIR}/src/detail/sequence.cpp + ${NETFT_XML_CONFIG_SOURCE}) +target_include_directories(netft_snapshot_client_impl_test PRIVATE + ${NETFT_SNAPSHOT_SOURCE_DIR}/src) +target_link_libraries(netft_snapshot_client_impl_test PUBLIC netft_core) + +add_executable(netft_snapshot_client_lifecycle + test_client_lifecycle.cpp + support/fake_http_server.cpp + support/fake_sensor.cpp) +target_include_directories(netft_snapshot_client_lifecycle PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} + ${NETFT_SNAPSHOT_SOURCE_DIR}/src) +target_link_libraries(netft_snapshot_client_lifecycle PRIVATE + netft_snapshot_client_impl_test + ${NETFT_GTEST_MAIN_TARGET}) +add_test(NAME netft_snapshot_client_lifecycle + COMMAND netft_snapshot_client_lifecycle) diff --git a/test/core/support/fake_http_server.cpp b/test/core/support/fake_http_server.cpp new file mode 100644 index 0000000..677f860 --- /dev/null +++ b/test/core/support/fake_http_server.cpp @@ -0,0 +1,199 @@ +#include "support/fake_http_server.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +void close_socket(int socket) noexcept { + if (socket >= 0) { + ::close(socket); + } +} + +void send_all(int socket, std::string_view data) noexcept { + while (!data.empty()) { + const auto sent = ::send(socket, data.data(), data.size(), MSG_NOSIGNAL); + if (sent <= 0) { + return; + } + data.remove_prefix(static_cast(sent)); + } +} + +} // namespace + +struct FakeHttpServer::Impl { + explicit Impl(std::string initial_body, int initial_status) + : body(std::move(initial_body)), status(initial_status) { + listener = ::socket(AF_INET, SOCK_STREAM, 0); + if (listener < 0) { + throw std::runtime_error("failed to create fake HTTP server socket"); + } + + const int reuse_address = 1; + ::setsockopt(listener, SOL_SOCKET, SO_REUSEADDR, &reuse_address, sizeof(reuse_address)); + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + address.sin_port = 0; + if (::bind(listener, reinterpret_cast(&address), sizeof(address)) != 0 || + ::listen(listener, 8) != 0) { + const auto error = std::string{std::strerror(errno)}; + close_socket(listener); + listener = -1; + throw std::runtime_error("failed to bind fake HTTP server: " + error); + } + + socklen_t address_length = sizeof(address); + if (::getsockname(listener, reinterpret_cast(&address), &address_length) != 0) { + const auto error = std::string{std::strerror(errno)}; + close_socket(listener); + listener = -1; + throw std::runtime_error("failed to inspect fake HTTP server: " + error); + } + listening_port = ntohs(address.sin_port); + worker = std::thread([this] { serve(); }); + } + + ~Impl() { + stopping.store(true); + response_changed.notify_all(); + if (listener >= 0) { + ::shutdown(listener, SHUT_RDWR); + close_socket(listener); + } + const int client = active_client.exchange(-1); + if (client >= 0) { + ::shutdown(client, SHUT_RDWR); + } + if (worker.joinable()) { + worker.join(); + } + } + + void serve() noexcept { + while (!stopping.load()) { + const int client = ::accept(listener, nullptr, nullptr); + if (client < 0) { + if (stopping.load()) { + return; + } + continue; + } + active_client.store(client); + accepted_connections.fetch_add(1); + if (stopping.load()) { + ::shutdown(client, SHUT_RDWR); + } + handle_request(client); + int expected_client = client; + static_cast(active_client.compare_exchange_strong(expected_client, -1)); + close_socket(client); + } + } + + void handle_request(int client) noexcept { + std::string request; + char buffer[1024]; + while (request.size() < 8192 && request.find("\r\n\r\n") == std::string::npos) { + const auto received = ::recv(client, buffer, sizeof(buffer), 0); + if (received <= 0) { + return; + } + request.append(buffer, static_cast(received)); + } + requests.fetch_add(1); + + std::string response_body; + std::string response_location; + int response_status{}; + std::chrono::milliseconds response_delay{}; + { + std::lock_guard lock(response_mutex); + response_body = body; + response_location = redirect_location; + response_status = status; + response_delay = delay; + } + + if (request.rfind("GET /netftapi2.xml ", 0) != 0) { + response_body.clear(); + response_status = 404; + } + + if (response_delay.count() > 0) { + std::unique_lock lock(response_mutex); + if (response_changed.wait_for(lock, response_delay, [this] { return stopping.load(); })) { + return; + } + } + + const std::string reason = response_status == 200 ? "OK" : "Error"; + const std::string location_header = + response_location.empty() ? std::string{} : "Location: " + response_location + "\r\n"; + const std::string headers = "HTTP/1.1 " + std::to_string(response_status) + " " + reason + + "\r\n" + "Content-Type: application/xml\r\n" + location_header + + "Content-Length: " + std::to_string(response_body.size()) + "\r\n" + + "Connection: close\r\n\r\n"; + send_all(client, headers); + send_all(client, response_body); + } + + std::string body; + std::string redirect_location; + int status; + std::chrono::milliseconds delay{}; + std::mutex response_mutex; + std::condition_variable response_changed; + std::atomic stopping{false}; + std::atomic requests{0}; + std::atomic accepted_connections{0}; + std::atomic active_client{-1}; + int listener{-1}; + int listening_port{}; + std::thread worker; +}; + +FakeHttpServer::FakeHttpServer(std::string body, int status) + : impl_(std::make_unique(std::move(body), status)) {} + +FakeHttpServer::~FakeHttpServer() = default; + +std::string FakeHttpServer::host() const { return "127.0.0.1"; } + +int FakeHttpServer::port() const { return impl_->listening_port; } + +std::uint64_t FakeHttpServer::request_count() const noexcept { return impl_->requests.load(); } + +std::uint64_t FakeHttpServer::accepted_connection_count() const noexcept { + return impl_->accepted_connections.load(); +} + +void FakeHttpServer::set_response(std::string body, int status) { + std::lock_guard lock(impl_->response_mutex); + impl_->body = std::move(body); + impl_->status = status; +} + +void FakeHttpServer::set_response_delay(std::chrono::milliseconds delay) { + std::lock_guard lock(impl_->response_mutex); + impl_->delay = delay; +} + +void FakeHttpServer::set_redirect_location(std::string location) { + std::lock_guard lock(impl_->response_mutex); + impl_->redirect_location = std::move(location); +} diff --git a/test/core/support/fake_http_server.hpp b/test/core/support/fake_http_server.hpp new file mode 100644 index 0000000..bd9eea0 --- /dev/null +++ b/test/core/support/fake_http_server.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include + +class FakeHttpServer { +public: + explicit FakeHttpServer(std::string body, int status = 200); + ~FakeHttpServer(); + + FakeHttpServer(const FakeHttpServer &) = delete; + FakeHttpServer &operator=(const FakeHttpServer &) = delete; + + std::string host() const; + int port() const; + std::uint64_t request_count() const noexcept; + std::uint64_t accepted_connection_count() const noexcept; + void set_response(std::string body, int status = 200); + void set_response_delay(std::chrono::milliseconds delay); + void set_redirect_location(std::string location); + +private: + struct Impl; + std::unique_ptr impl_; +}; diff --git a/test/core/support/fake_sensor.cpp b/test/core/support/fake_sensor.cpp new file mode 100644 index 0000000..17005d5 --- /dev/null +++ b/test/core/support/fake_sensor.cpp @@ -0,0 +1,260 @@ +#include "support/fake_sensor.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace netft::test { +namespace { + +constexpr std::string_view kDefaultXml = R"xml( +Fake Net F/T1000000 +1000000NNm)xml"; + +void put_u32(std::vector &bytes, std::size_t offset, std::uint32_t value) { + for (unsigned index = 0; index < 4; ++index) { + bytes[offset + index] = static_cast(value >> (24U - 8U * index)); + } +} + +std::vector make_record(const std::uint32_t rdt_sequence, const std::uint32_t status, + const std::uint32_t ft_sequence, + const std::array &axes) { + std::vector data(36); + put_u32(data, 0, rdt_sequence); + put_u32(data, 4, ft_sequence); + put_u32(data, 8, status); + for (std::size_t index = 0; index < axes.size(); ++index) { + put_u32(data, 12 + 4 * index, static_cast(axes[index])); + } + return data; +} + +} // namespace + +FakeSensor::FakeSensor(const double rate_hz) : http_(std::string{kDefaultXml}), rate_hz_(rate_hz) { + socket_ = ::socket(AF_INET, SOCK_DGRAM, 0); + if (socket_ < 0) { + throw std::runtime_error("fake sensor socket failed"); + } + const int flags = ::fcntl(socket_, F_GETFL, 0); + static_cast(::fcntl(socket_, F_SETFL, flags | O_NONBLOCK)); + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + if (::bind(socket_, reinterpret_cast(&address), sizeof(address)) != 0) { + ::close(socket_); + socket_ = -1; + throw std::runtime_error("fake sensor bind failed"); + } + socklen_t address_size = sizeof(address); + if (::getsockname(socket_, reinterpret_cast(&address), &address_size) != 0) { + ::close(socket_); + socket_ = -1; + throw std::runtime_error("fake sensor getsockname failed"); + } + rdt_port_ = ntohs(address.sin_port); + worker_ = std::thread(&FakeSensor::run, this); +} + +FakeSensor::~FakeSensor() { + stopping_ = true; + if (socket_ >= 0) { + ::shutdown(socket_, SHUT_RDWR); + } + if (worker_.joinable()) { + worker_.join(); + } + if (socket_ >= 0) { + ::close(socket_); + } +} + +void FakeSensor::pause() noexcept { enabled_ = false; } + +void FakeSensor::resume() noexcept { enabled_ = true; } + +void FakeSensor::queue_payload(std::vector payload) { + std::lock_guard lock(mutex_); + payloads_.push_back(std::move(payload)); +} + +void FakeSensor::send_payload_now(std::vector payload) { + ::sockaddr_in peer{}; + { + std::lock_guard lock(mutex_); + if (!has_client_) { + throw std::runtime_error("fake sensor has no client"); + } + peer = client_; + } + static_cast(::sendto(socket_, payload.data(), payload.size(), 0, + reinterpret_cast(&peer), sizeof(peer))); +} + +void FakeSensor::queue_record(const std::uint32_t rdt_sequence, const std::uint32_t status, + std::uint32_t ft_sequence, const std::array axes) { + std::lock_guard lock(mutex_); + if (ft_sequence == 0) { + ft_sequence = ft_; + } + ft_ = ft_sequence + 4; + payloads_.push_back(make_record(rdt_sequence, status, ft_sequence, axes)); +} + +void FakeSensor::send_record_now(const std::uint32_t rdt_sequence, const std::uint32_t status, + std::uint32_t ft_sequence, + const std::array axes) { + { + std::lock_guard lock(mutex_); + if (ft_sequence == 0) { + ft_sequence = ft_; + } + ft_ = ft_sequence + 4; + } + send_payload_now(make_record(rdt_sequence, status, ft_sequence, axes)); +} + +void FakeSensor::skip_rdt(const unsigned count) noexcept { + std::lock_guard lock(mutex_); + skip_ += count; +} + +bool FakeSensor::wait_for_command(const detail::Command command, const unsigned count, + const std::chrono::milliseconds timeout) const { + const auto deadline = std::chrono::steady_clock::now() + timeout; + do { + const auto observed = commands(); + if (std::count(observed.begin(), observed.end(), command) >= + static_cast(count)) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds{2}); + } while (std::chrono::steady_clock::now() < deadline); + return false; +} + +bool FakeSensor::wait_for_http_request(const unsigned count, + const std::chrono::milliseconds timeout) const { + const auto deadline = std::chrono::steady_clock::now() + timeout; + do { + if (http_request_count() >= count) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds{2}); + } while (std::chrono::steady_clock::now() < deadline); + return false; +} + +std::vector FakeSensor::commands() const { + std::lock_guard lock(mutex_); + return commands_; +} + +std::vector FakeSensor::command_events() const { + std::lock_guard lock(mutex_); + return command_events_; +} + +void FakeSensor::set_xml_configuration(std::string xml, const int status) { + http_.set_response(std::move(xml), status); +} + +void FakeSensor::set_http_response_delay(const std::chrono::milliseconds delay) { + http_.set_response_delay(delay); +} + +void FakeSensor::record_command(const std::uint8_t *data, const std::size_t size, + const ::sockaddr_in &peer) { + if (size != 8 || data[0] != 0x12 || data[1] != 0x34) { + return; + } + const auto command = + static_cast((static_cast(data[2]) << 8U) | data[3]); + { + std::lock_guard lock(mutex_); + commands_.push_back(command); + command_events_.push_back({command, std::chrono::steady_clock::now()}); + if (command == detail::Command::StartRealtime) { + client_ = peer; + has_client_ = true; + rdt_ = 0; + } + } + if (command == detail::Command::StartRealtime) { + streaming_ = true; + } + if (command == detail::Command::StopStreaming || command == detail::Command::SetSoftwareBias) { + streaming_ = false; + } +} + +void FakeSensor::send_next() { + if (!streaming_ || !enabled_) { + return; + } + std::vector data; + ::sockaddr_in peer{}; + { + std::lock_guard lock(mutex_); + if (!has_client_) { + return; + } + peer = client_; + if (!payloads_.empty()) { + data = std::move(payloads_.front()); + payloads_.erase(payloads_.begin()); + } + if (data.empty()) { + data.resize(36); + rdt_ += 1 + skip_; + skip_ = 0; + put_u32(data, 0, rdt_); + put_u32(data, 4, ft_); + ft_ += 4; + put_u32(data, 12, 100); + put_u32(data, 16, static_cast(-200)); + put_u32(data, 20, 300); + put_u32(data, 24, 10); + put_u32(data, 28, static_cast(-20)); + put_u32(data, 32, 30); + } + } + static_cast(::sendto(socket_, data.data(), data.size(), 0, + reinterpret_cast(&peer), sizeof(peer))); +} + +void FakeSensor::run() { + auto next = std::chrono::steady_clock::now(); + const auto interval = std::chrono::duration_cast( + std::chrono::duration{1.0 / rate_hz_}); + while (!stopping_) { + ::sockaddr_in peer{}; + socklen_t peer_size = sizeof(peer); + std::array data{}; + const auto size = ::recvfrom(socket_, data.data(), data.size(), 0, + reinterpret_cast(&peer), &peer_size); + if (size > 0) { + record_command(data.data(), static_cast(size), peer); + } + const auto now = std::chrono::steady_clock::now(); + if (now >= next) { + send_next(); + next += interval; + if (next < now) { + next = now + interval; + } + } + std::this_thread::sleep_for(std::chrono::microseconds{100}); + } +} + +} // namespace netft::test diff --git a/test/core/support/fake_sensor.hpp b/test/core/support/fake_sensor.hpp new file mode 100644 index 0000000..97cb61b --- /dev/null +++ b/test/core/support/fake_sensor.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "detail/protocol.hpp" +#include "support/fake_http_server.hpp" + +namespace netft::test { + +class FakeSensor { +public: + struct CommandEvent { + detail::Command command; + std::chrono::steady_clock::time_point at; + }; + + explicit FakeSensor(double rate_hz = 200.0); + ~FakeSensor(); + FakeSensor(const FakeSensor &) = delete; + FakeSensor &operator=(const FakeSensor &) = delete; + + const std::string &host() const noexcept { return host_; } + int rdt_port() const noexcept { return rdt_port_; } + int http_port() const noexcept { return http_.port(); } + std::uint64_t http_request_count() const noexcept { return http_.request_count(); } + + void pause() noexcept; + void resume() noexcept; + void queue_record(std::uint32_t rdt_sequence, std::uint32_t status = 0, + std::uint32_t ft_sequence = 0, + std::array axes = {100, -200, 300, 10, -20, 30}); + void send_record_now(std::uint32_t rdt_sequence, std::uint32_t status = 0, + std::uint32_t ft_sequence = 0, + std::array axes = {100, -200, 300, 10, -20, 30}); + void queue_payload(std::vector payload); + void send_payload_now(std::vector payload); + void skip_rdt(unsigned count) noexcept; + bool wait_for_command(detail::Command command, unsigned count = 1, + std::chrono::milliseconds timeout = std::chrono::milliseconds{1000}) const; + bool wait_for_http_request(unsigned count = 1, + std::chrono::milliseconds timeout = std::chrono::milliseconds{ + 1000}) const; + std::vector commands() const; + std::vector command_events() const; + void set_xml_configuration(std::string xml, int status = 200); + void set_http_response_delay(std::chrono::milliseconds delay); + +private: + void run(); + void send_next(); + void record_command(const std::uint8_t *data, std::size_t size, const ::sockaddr_in &peer); + + std::string host_{"127.0.0.1"}; + FakeHttpServer http_; + int rdt_port_{}; + int socket_{-1}; + double rate_hz_{}; + std::atomic stopping_{false}; + std::atomic enabled_{true}; + std::atomic streaming_{false}; + std::thread worker_; + mutable std::mutex mutex_; + std::vector commands_; + std::vector command_events_; + std::vector> payloads_; + ::sockaddr_in client_{}; + bool has_client_{false}; + std::uint32_t rdt_{}; + std::uint32_t ft_{1000}; + std::uint32_t skip_{}; +}; + +} // namespace netft::test diff --git a/test/core/test_client_lifecycle.cpp b/test/core/test_client_lifecycle.cpp new file mode 100644 index 0000000..850ef10 --- /dev/null +++ b/test/core/test_client_lifecycle.cpp @@ -0,0 +1,714 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "detail/protocol.hpp" +#define private public +#include "detail/client_impl.hpp" +#undef private +#include "support/fake_sensor.hpp" + +namespace { + +using namespace std::chrono_literals; + +constexpr auto kChangedConfiguration = R"xml( +Fake Net F/T1000000 +1000NN-mm)xml"; + +netft::Config config_for(const netft::test::FakeSensor &sensor) { + netft::Config config; + config.sensor_host = sensor.host(); + config.rdt_port = sensor.rdt_port(); + config.http_port = sensor.http_port(); + config.receive_timeout = 120ms; + config.configuration_connect_timeout = 100ms; + config.configuration_timeout = 250ms; + config.reconnect_initial_delay = 5ms; + config.reconnect_max_delay = 20ms; + return config; +} + +template +bool wait_until(Predicate predicate, const std::chrono::milliseconds timeout = 1s) { + const auto deadline = std::chrono::steady_clock::now() + timeout; + do { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(2ms); + } while (std::chrono::steady_clock::now() < deadline); + return predicate(); +} + +struct CopyThrowState { + std::atomic throw_on_copy{}; + std::atomic copy_attempts{}; + std::atomic calls{}; +}; + +struct CopyThrowingCallback { + explicit CopyThrowingCallback(std::shared_ptr state_in) + : state{std::move(state_in)} {} + + CopyThrowingCallback(const CopyThrowingCallback &other) : state{other.state} { + ++state->copy_attempts; + if (state->throw_on_copy) { + throw std::bad_alloc{}; + } + } + CopyThrowingCallback(CopyThrowingCallback &&) noexcept = default; + + void operator()(const netft::Sample &) const { ++state->calls; } + + std::shared_ptr state; +}; + +struct HookGate { + std::mutex mutex; + std::condition_variable cv; + bool entered{}; + bool released{}; +}; + +struct ClientLifecycleTestAccess { + static void fail_next_thread_creation(netft::Client &client) { + fail_next = true; + client.impl_->thread_factory_ = &create_thread; + } + + static bool worker_joinable(const netft::Client &client) { + return client.impl_->worker_.joinable(); + } + + static bool worker_exited(const netft::Client &client) { return client.impl_->worker_exited_; } + + static bool has_callback(const netft::Client &client) { + return static_cast(client.impl_->callback_); + } + + static bool stopping(const netft::Client &client) { return client.impl_->stopping_; } + + static std::uint64_t generation(const netft::Client &client) { + std::lock_guard data_lock(client.impl_->data_mutex_); + return client.impl_->generation_; + } + + static void gate_fault_publication(netft::Client &client) { + reset_gate(fault_gate); + client.impl_->fault_published_test_hook_ = &on_fault_published; + } + + static bool wait_for_fault_publication(const std::chrono::milliseconds timeout = 1s) { + return wait_for_gate(fault_gate, timeout); + } + + static void release_fault_publication() { release_gate(fault_gate); } + + static void gate_waiter_after_wake(netft::Client &client) { + reset_gate(waiter_gate); + client.impl_->wait_wake_test_hook_ = &on_wait_wake; + } + + static bool wait_for_waiter_wake(const std::chrono::milliseconds timeout = 1s) { + return wait_for_gate(waiter_gate, timeout); + } + + static void release_waiter() { release_gate(waiter_gate); } + +private: + static std::thread create_thread(netft::Client::Impl *impl) { + if (fail_next.exchange(false)) { + throw std::system_error{std::make_error_code(std::errc::resource_unavailable_try_again), + "injected thread creation failure"}; + } + return std::thread{&netft::Client::Impl::run, impl}; + } + + static void reset_gate(HookGate &gate) { + std::lock_guard lock(gate.mutex); + gate.entered = false; + gate.released = false; + } + + static bool wait_for_gate(HookGate &gate, const std::chrono::milliseconds timeout) { + std::unique_lock lock(gate.mutex); + return gate.cv.wait_for(lock, timeout, [&] { return gate.entered; }); + } + + static void release_gate(HookGate &gate) { + { + std::lock_guard lock(gate.mutex); + gate.released = true; + } + gate.cv.notify_all(); + } + + static void enter_gate(HookGate &gate) noexcept { + try { + std::unique_lock lock(gate.mutex); + gate.entered = true; + gate.cv.notify_all(); + static_cast(gate.cv.wait_for(lock, 2s, [&] { return gate.released; })); + } catch (...) { + } + } + + static void on_fault_published(netft::Client::Impl *) noexcept { enter_gate(fault_gate); } + + static void on_wait_wake(netft::Client::Impl *, std::uint64_t) noexcept { + enter_gate(waiter_gate); + } + + static inline std::atomic fail_next{}; + static inline HookGate fault_gate; + static inline HookGate waiter_gate; +}; + +TEST(ClientLifecycle, RateLimitDropsIntermediateSamplesWithoutQueueing) { + netft::test::FakeSensor sensor{1000.0}; + auto config = config_for(sensor); + config.sample_rate_limit_hz = 50.0; + netft::Client client{config}; + std::atomic delivered{}; + client.start([&](const netft::Sample &) { ++delivered; }); + + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + ASSERT_TRUE(wait_until([&] { + const auto health = client.health(); + return health.delivered_count >= 3 && health.rate_limited_count > 0; + })); + client.stop(); + + const auto health = client.health(); + EXPECT_GT(health.delivered_count, 0U); + EXPECT_GT(health.received_count, health.delivered_count); + EXPECT_GT(health.rate_limited_count, 0U); + EXPECT_EQ(health.delivered_count, delivered.load()); + EXPECT_EQ(health.received_count, health.delivered_count + health.rate_limited_count); + EXPECT_DOUBLE_EQ(health.delivery_rate_hz, static_cast(health.delivered_count)); +} + +TEST(ClientLifecycle, CallbackExceptionsContinueUnderReconnectPolicy) { + netft::test::FakeSensor sensor{200.0}; + netft::Client client{config_for(sensor)}; + std::atomic calls{}; + client.start([&](const netft::Sample &) { + ++calls; + throw std::runtime_error{"consumer failed"}; + }); + + ASSERT_TRUE(wait_until([&] { return client.health().callback_error_count >= 3; })); + EXPECT_FALSE(client.faulted()); + EXPECT_GE(calls.load(), 3U); + EXPECT_EQ(client.health().delivered_count, 0U); + client.stop(); +} + +TEST(ClientLifecycle, CallbackCopyExceptionsContinueUnderReconnectPolicy) { + netft::test::FakeSensor sensor{200.0}; + sensor.pause(); + netft::Client client{config_for(sensor)}; + auto state = std::make_shared(); + netft::Client::SampleCallback callback{CopyThrowingCallback{state}}; + client.start(std::move(callback)); + ASSERT_EQ(state->copy_attempts.load(), 0U); + + state->throw_on_copy = true; + sensor.resume(); + ASSERT_TRUE(wait_until([&] { return client.health().callback_error_count >= 3; })); + EXPECT_EQ(state->calls.load(), 0U); + EXPECT_FALSE(client.faulted()); + + state->throw_on_copy = false; + ASSERT_TRUE(wait_until([&] { return state->calls.load() > 0; })); + EXPECT_TRUE(client.wait_for_first_sample(100ms)); + client.stop(); +} + +TEST(ClientLifecycle, CallbackExceptionLatchesUnderFailStopPolicy) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + std::atomic calls{}; + client.start([&](const netft::Sample &) { + ++calls; + throw std::runtime_error{"fail-stop callback"}; + }); + sensor.queue_record(1, 0, 100); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::Callback); + EXPECT_EQ(calls.load(), 1U); + EXPECT_EQ(client.health().callback_error_count, 1U); + EXPECT_EQ(client.health().reconnect_count, 0U); + client.stop(); + EXPECT_EQ(client.fault_code(), netft::FaultCode::Callback); +} + +TEST(ClientLifecycle, CallbackCopyExceptionLatchesUnderFailStopPolicy) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + auto state = std::make_shared(); + netft::Client::SampleCallback callback{CopyThrowingCallback{state}}; + client.start(std::move(callback)); + ASSERT_EQ(state->copy_attempts.load(), 0U); + state->throw_on_copy = true; + sensor.queue_record(1, 0, 100); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::Callback); + EXPECT_EQ(state->copy_attempts.load(), 1U); + EXPECT_EQ(state->calls.load(), 0U); + EXPECT_EQ(client.health().callback_error_count, 1U); + EXPECT_EQ(client.health().reconnect_count, 0U); + client.stop(); +} + +TEST(ClientLifecycle, EarlierSeriousStatusWinsOverCallbackFailure) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.deliver_samples_with_error_status = true; + netft::Client client{config}; + client.start([](const netft::Sample &) { throw std::runtime_error{"serious callback failed"}; }); + sensor.queue_record(1, 0x80020000U, 100); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::SeriousStatus); + EXPECT_EQ(client.health().callback_error_count, 1U); + EXPECT_EQ(client.health().device_error_count, 1U); + client.stop(); +} + +TEST(ClientLifecycle, CallbackFailureDoesNotConsumeDeliveryRateSlot) { + netft::test::FakeSensor sensor{500.0}; + auto config = config_for(sensor); + config.sample_rate_limit_hz = 1.0; + netft::Client client{config}; + std::atomic calls{}; + client.start([&](const netft::Sample &) { + if (++calls == 1) { + sensor.pause(); + throw std::runtime_error{"first delivery failed"}; + } + }); + + EXPECT_FALSE(client.wait_for_first_sample(50ms)); + sensor.resume(); + ASSERT_TRUE(wait_until([&] { return calls.load() >= 2; }, 300ms)); + EXPECT_EQ(client.health().delivered_count, 1U); + client.stop(); +} + +TEST(ClientLifecycle, BiasRequiresStreamingAndRestartsRealtime) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 2s; + netft::Client client{config}; + EXPECT_THROW(client.bias(), netft::NotConnectedError); + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + EXPECT_EQ(client.health().state, netft::ClientState::Connecting); + EXPECT_THROW(client.bias(), netft::NotConnectedError); + + sensor.resume(); + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + EXPECT_NO_THROW(client.bias()); + EXPECT_TRUE(sensor.wait_for_command(netft::detail::Command::SetSoftwareBias)); + EXPECT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 2)); + client.stop(); +} + +TEST(ClientLifecycle, CallbackCanReenterBias) { + netft::test::FakeSensor sensor{200.0}; + netft::Client client{config_for(sensor)}; + std::atomic calls{}; + client.start([&](const netft::Sample &) { + ++calls; + client.bias(); + }); + + ASSERT_TRUE(wait_until([&] { return calls.load() >= 2 && sensor.commands().size() >= 5; })); + client.stop(); +} + +TEST(ClientLifecycle, StartRejectsActiveClientAndStopIsIdempotent) { + netft::test::FakeSensor sensor; + netft::Client client{config_for(sensor)}; + client.start([](const netft::Sample &) {}); + EXPECT_THROW(client.start([](const netft::Sample &) {}), std::logic_error); + client.stop(); + client.stop(); +} + +TEST(ClientLifecycle, CallbackStopReturnsAndExternalStopReapsWorker) { + netft::test::FakeSensor sensor{200.0}; + netft::Client client{config_for(sensor)}; + std::atomic returned{}; + client.start([&](const netft::Sample &) { + client.stop(); + returned = true; + }); + + ASSERT_TRUE(wait_until([&] { return returned.load(); })); + ASSERT_TRUE(wait_until([&] { return ClientLifecycleTestAccess::worker_exited(client); })); + EXPECT_TRUE(ClientLifecycleTestAccess::worker_joinable(client)); + client.stop(); + EXPECT_FALSE(ClientLifecycleTestAccess::worker_joinable(client)); + EXPECT_EQ(client.health().state, netft::ClientState::Stopped); + + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + client.stop(); + EXPECT_FALSE(ClientLifecycleTestAccess::worker_joinable(client)); +} + +TEST(ClientLifecycle, CallbackStartAndConcurrentStopDoNotDeadlock) { + netft::test::FakeSensor sensor{200.0}; + netft::Client client{config_for(sensor)}; + std::atomic entered{}; + std::atomic stop_started{}; + std::atomic callback_finished{}; + client.start([&](const netft::Sample &) { + entered = true; + while (!stop_started) { + std::this_thread::yield(); + } + EXPECT_THROW(client.start([](const netft::Sample &) {}), std::logic_error); + callback_finished = true; + }); + ASSERT_TRUE(wait_until([&] { return entered.load(); })); + + std::thread stopper([&] { + stop_started = true; + client.stop(); + }); + ASSERT_TRUE(wait_until([&] { return callback_finished.load(); }, 300ms)); + stopper.join(); + client.stop(); + EXPECT_EQ(client.health().state, netft::ClientState::Stopped); +} + +TEST(ClientLifecycle, CallbackStoppedWorkerCanBeReapedByDirectRestart) { + netft::test::FakeSensor sensor{200.0}; + netft::Client client{config_for(sensor)}; + std::atomic stopped{}; + client.start([&](const netft::Sample &) { + client.stop(); + stopped = true; + }); + ASSERT_TRUE(wait_until([&] { return stopped.load(); })); + ASSERT_TRUE(wait_until([&] { return client.health().state == netft::ClientState::Stopped; })); + ASSERT_TRUE(wait_until([&] { return ClientLifecycleTestAccess::worker_exited(client); })); + EXPECT_TRUE(ClientLifecycleTestAccess::worker_joinable(client)); + const auto stopped_generation = ClientLifecycleTestAccess::generation(client); + + std::atomic restarted{}; + client.start([&](const netft::Sample &) { ++restarted; }); + EXPECT_EQ(ClientLifecycleTestAccess::generation(client), stopped_generation + 1); + ASSERT_TRUE(wait_until([&] { return restarted.load() > 0; })); + client.stop(); + EXPECT_FALSE(ClientLifecycleTestAccess::worker_joinable(client)); +} + +TEST(ClientLifecycle, FaultedWorkerCanBeDirectlyRestartedBeforeItExits) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + ClientLifecycleTestAccess::gate_fault_publication(client); + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + const auto faulted_generation = ClientLifecycleTestAccess::generation(client); + sensor.send_record_now(1, 0x80020000U, 100); + + ASSERT_TRUE(ClientLifecycleTestAccess::wait_for_fault_publication()); + ASSERT_TRUE(client.faulted()); + ASSERT_FALSE(ClientLifecycleTestAccess::worker_exited(client)); + ASSERT_TRUE(ClientLifecycleTestAccess::worker_joinable(client)); + + std::atomic restart_entered{}; + std::atomic restart_returned{}; + std::exception_ptr restart_error; + std::atomic restarted_deliveries{}; + std::thread restarter([&] { + restart_entered = true; + try { + client.start([&](const netft::Sample &) { ++restarted_deliveries; }); + } catch (...) { + restart_error = std::current_exception(); + } + restart_returned = true; + }); + EXPECT_TRUE(wait_until([&] { return restart_entered.load(); })); + EXPECT_FALSE(wait_until([&] { return restart_returned.load(); }, 20ms)); + + ClientLifecycleTestAccess::release_fault_publication(); + restarter.join(); + EXPECT_EQ(restart_error, nullptr); + EXPECT_TRUE(restart_returned.load()); + EXPECT_EQ(ClientLifecycleTestAccess::generation(client), faulted_generation + 1); + EXPECT_FALSE(client.faulted()); + + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 2)); + sensor.send_record_now(1, 0, 104); + EXPECT_TRUE(wait_until([&] { return restarted_deliveries.load() == 1; }, 500ms)); + client.stop(); +} + +TEST(ClientLifecycle, RestartClearsActiveStateAndPreservesLifetimeHealth) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.deliver_samples_with_error_status = true; + config.receive_timeout = 2s; + netft::Client client{config}; + + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + sensor.send_record_now(17, 0x80020000U, 100); + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + ASSERT_TRUE(wait_until([&] { return ClientLifecycleTestAccess::worker_exited(client); })); + client.stop(); + + const auto before_restart = client.health(); + ASSERT_TRUE(client.latest_sample()); + ASSERT_TRUE(before_restart.sensor_configuration); + ASSERT_EQ(before_restart.fault_code, netft::FaultCode::SeriousStatus); + ASSERT_EQ(before_restart.received_count, 1U); + ASSERT_EQ(before_restart.delivered_count, 1U); + ASSERT_EQ(before_restart.device_error_count, 1U); + ASSERT_EQ(before_restart.sensor_configuration->revision, 1U); + + sensor.set_xml_configuration(kChangedConfiguration); + client.start([](const netft::Sample &) {}); + + const auto starting = client.health(); + EXPECT_FALSE(client.faulted()); + EXPECT_EQ(starting.fault_code, netft::FaultCode::None); + EXPECT_FALSE(client.latest_sample()); + EXPECT_EQ(starting.state, netft::ClientState::Connecting); + EXPECT_TRUE(starting.last_error.empty()); + EXPECT_DOUBLE_EQ(starting.receive_rate_hz, 0.0); + EXPECT_DOUBLE_EQ(starting.delivery_rate_hz, 0.0); + EXPECT_EQ(starting.received_count, before_restart.received_count); + EXPECT_EQ(starting.delivered_count, before_restart.delivered_count); + EXPECT_EQ(starting.device_error_count, before_restart.device_error_count); + + ASSERT_TRUE(sensor.wait_for_http_request(2)); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 2)); + const auto rediscovered = client.health(); + ASSERT_TRUE(rediscovered.sensor_configuration); + EXPECT_EQ(rediscovered.sensor_configuration->revision, 2U); + EXPECT_EQ(rediscovered.calibration_change_count, before_restart.calibration_change_count + 1); + EXPECT_EQ(rediscovered.received_count, before_restart.received_count); + EXPECT_EQ(rediscovered.delivered_count, before_restart.delivered_count); + + sensor.send_record_now(1, 0, 104); + ASSERT_TRUE(wait_until( + [&] { return client.health().delivered_count == before_restart.delivered_count + 1; })); + const auto after_restart = client.health(); + const auto latest = client.latest_sample(); + ASSERT_TRUE(latest); + EXPECT_EQ(latest->configuration_revision, 2U); + EXPECT_EQ(latest->torque_unit, netft::TorqueUnit::NewtonMillimeter); + EXPECT_EQ(after_restart.received_count, before_restart.received_count + 1); + EXPECT_EQ(after_restart.delivered_count, before_restart.delivered_count + 1); + EXPECT_EQ(after_restart.device_error_count, before_restart.device_error_count); + client.stop(); +} + +TEST(ClientLifecycle, ConcurrentStartAndStopLeaveClientRestartable) { + netft::test::FakeSensor sensor{400.0}; + netft::Client client{config_for(sensor)}; + for (int iteration = 0; iteration < 40; ++iteration) { + std::thread starter([&] { + try { + client.start([](const netft::Sample &) {}); + } catch (const std::logic_error &) { + } + }); + std::thread stopper([&] { client.stop(); }); + starter.join(); + stopper.join(); + client.stop(); + } + + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(wait_until([&] { return client.health().received_count > 0; })); + client.stop(); + EXPECT_EQ(client.health().state, netft::ClientState::Stopped); +} + +TEST(ClientLifecycle, ThreadCreationFailureRestoresPriorFaultedState) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.deliver_samples_with_error_status = true; + netft::Client client{config}; + std::atomic original_callback_calls{}; + client.start([&](const netft::Sample &) { ++original_callback_calls; }); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + sensor.queue_record(17, 0x80020000U, 100); + sensor.resume(); + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + ASSERT_TRUE(wait_until([&] { return ClientLifecycleTestAccess::worker_exited(client); })); + + const auto prior_health = client.health(); + const auto prior_latest = client.latest_sample(); + const auto prior_generation = ClientLifecycleTestAccess::generation(client); + ASSERT_TRUE(prior_latest); + ASSERT_EQ(original_callback_calls.load(), 1U); + ASSERT_EQ(prior_health.state, netft::ClientState::Faulted); + ASSERT_EQ(prior_health.fault_code, netft::FaultCode::SeriousStatus); + ASSERT_EQ(prior_health.received_count, 1U); + ASSERT_EQ(prior_health.delivered_count, 1U); + + ClientLifecycleTestAccess::fail_next_thread_creation(client); + + EXPECT_THROW(client.start([](const netft::Sample &) {}), std::system_error); + const auto restored_health = client.health(); + const auto restored_latest = client.latest_sample(); + ASSERT_TRUE(restored_latest); + EXPECT_EQ(restored_health.state, prior_health.state); + EXPECT_EQ(restored_health.fault_code, prior_health.fault_code); + EXPECT_EQ(restored_health.last_error, prior_health.last_error); + EXPECT_EQ(restored_health.received_count, prior_health.received_count); + EXPECT_EQ(restored_health.delivered_count, prior_health.delivered_count); + EXPECT_EQ(restored_health.device_error_count, prior_health.device_error_count); + EXPECT_EQ(restored_health.delivery_rate_hz, prior_health.delivery_rate_hz); + EXPECT_EQ(restored_latest->rdt_sequence, prior_latest->rdt_sequence); + EXPECT_EQ(restored_latest->ft_sequence, prior_latest->ft_sequence); + EXPECT_EQ(client.fault_code(), netft::FaultCode::SeriousStatus); + EXPECT_FALSE(ClientLifecycleTestAccess::worker_joinable(client)); + EXPECT_TRUE(ClientLifecycleTestAccess::worker_exited(client)); + EXPECT_TRUE(ClientLifecycleTestAccess::has_callback(client)); + EXPECT_FALSE(ClientLifecycleTestAccess::stopping(client)); + EXPECT_EQ(ClientLifecycleTestAccess::generation(client), prior_generation); + EXPECT_TRUE(client.wait_for_first_sample(10ms)); + client.stop(); +} + +TEST(ClientLifecycle, WaiterWakesForStopAndFault) { + { + netft::test::FakeSensor sensor; + sensor.pause(); + netft::Client client{config_for(sensor)}; + std::atomic returned{}; + client.start([](const netft::Sample &) {}); + std::thread waiter([&] { + EXPECT_FALSE(client.wait_for_first_sample(2s)); + returned = true; + }); + std::this_thread::sleep_for(10ms); + client.stop(); + ASSERT_TRUE(wait_until([&] { return returned.load(); }, 200ms)); + waiter.join(); + } + { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.receive_timeout = 50ms; + netft::Client client{config}; + std::atomic returned{}; + client.start([](const netft::Sample &) {}); + std::thread waiter([&] { + EXPECT_FALSE(client.wait_for_first_sample(2s)); + returned = true; + }); + ASSERT_TRUE(wait_until([&] { return client.faulted(); }, 500ms)); + ASSERT_TRUE(wait_until([&] { return returned.load(); }, 200ms)); + waiter.join(); + client.stop(); + } +} + +TEST(ClientLifecycle, OldWaiterReturnsFalseAfterNewGenerationDelivers) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 2s; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ClientLifecycleTestAccess::gate_waiter_after_wake(client); + std::atomic waiter_entered{}; + std::atomic waiter_returned{}; + std::atomic waiter_result{true}; + std::thread waiter([&] { + waiter_entered = true; + waiter_result = client.wait_for_first_sample(2s); + waiter_returned = true; + }); + ASSERT_TRUE(wait_until([&] { return waiter_entered.load(); })); + + client.stop(); + const bool waiter_woke = ClientLifecycleTestAccess::wait_for_waiter_wake(); + client.start([](const netft::Sample &) {}); + sensor.resume(); + const bool new_generation_delivered = + wait_until([&] { return client.health().delivered_count > 0; }, 500ms); + EXPECT_FALSE(waiter_returned.load()); + + ClientLifecycleTestAccess::release_waiter(); + waiter.join(); + EXPECT_TRUE(waiter_woke); + EXPECT_TRUE(new_generation_delivered); + EXPECT_TRUE(waiter_returned.load()); + EXPECT_FALSE(waiter_result.load()); + client.stop(); +} + +TEST(ClientLifecycle, WaitForFirstSampleRequiresSuccessfulDelivery) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + client.start([](const netft::Sample &) { throw std::runtime_error{"delivery failed"}; }); + sensor.queue_record(1, 0, 100); + sensor.resume(); + + EXPECT_FALSE(client.wait_for_first_sample(500ms)); + EXPECT_EQ(client.fault_code(), netft::FaultCode::Callback); + client.stop(); +} + +TEST(ClientLifecycle, DestructionSendsStopStreaming) { + netft::test::FakeSensor sensor; + { + netft::Client client{config_for(sensor)}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(wait_until([&] { return client.health().received_count > 0; })); + } + EXPECT_TRUE(sensor.wait_for_command(netft::detail::Command::StopStreaming)); +} + +} // namespace diff --git a/test/core/test_client_recovery.cpp b/test/core/test_client_recovery.cpp new file mode 100644 index 0000000..3275fab --- /dev/null +++ b/test/core/test_client_recovery.cpp @@ -0,0 +1,543 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "detail/protocol.hpp" +#include "netft/client.hpp" +#include "support/fake_sensor.hpp" + +namespace { + +using namespace std::chrono_literals; + +constexpr auto kChangedConfiguration = R"xml( +Fake Net F/T1000000 +1000NN-mm)xml"; + +netft::Config config_for(const netft::test::FakeSensor &sensor) { + netft::Config config; + config.sensor_host = sensor.host(); + config.rdt_port = sensor.rdt_port(); + config.http_port = sensor.http_port(); + config.receive_timeout = 120ms; + config.configuration_connect_timeout = 100ms; + config.configuration_timeout = 250ms; + config.reconnect_initial_delay = 10ms; + config.reconnect_max_delay = 40ms; + return config; +} + +template +bool wait_until(Predicate predicate, const std::chrono::milliseconds timeout = 1s) { + const auto deadline = std::chrono::steady_clock::now() + timeout; + do { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(2ms); + } while (std::chrono::steady_clock::now() < deadline); + return predicate(); +} + +std::size_t start_count(const netft::test::FakeSensor &sensor) { + const auto commands = sensor.commands(); + return static_cast( + std::count(commands.begin(), commands.end(), netft::detail::Command::StartRealtime)); +} + +std::vector +start_times(const netft::test::FakeSensor &sensor) { + std::vector times; + for (const auto &event : sensor.command_events()) { + if (event.command == netft::detail::Command::StartRealtime) { + times.push_back(event.at); + } + } + return times; +} + +TEST(ClientRecovery, CountsLossDuplicatesAndOutOfOrderBeforeOrderedDelivery) { + netft::test::FakeSensor sensor{500.0}; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.receive_timeout = 1s; + std::mutex delivered_mutex; + std::vector delivered; + netft::Client client{config}; + client.start([&](const netft::Sample &sample) { + std::lock_guard lock(delivered_mutex); + delivered.push_back(sample.rdt_sequence); + }); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + sensor.send_record_now(1, 0, 100); + ASSERT_TRUE(wait_until([&] { return client.health().received_count == 1; })); + sensor.send_record_now(4, 0, 104); + ASSERT_TRUE(wait_until([&] { return client.health().received_count == 2; })); + sensor.send_record_now(4, 0x80010000U, 108); + ASSERT_TRUE(wait_until([&] { return client.health().received_count == 3; })); + sensor.send_record_now(3, 0x80010000U, 112); + ASSERT_TRUE(wait_until([&] { return client.health().received_count == 4; })); + client.stop(); + const auto health = client.health(); + std::lock_guard lock(delivered_mutex); + EXPECT_EQ(delivered, (std::vector{1, 4})); + EXPECT_EQ(health.received_count, 4U); + EXPECT_EQ(health.delivered_count, 2U); + EXPECT_EQ(health.lost_count, 2U); + EXPECT_EQ(health.duplicate_count, 1U); + EXPECT_EQ(health.out_of_order_count, 1U); + EXPECT_EQ(health.warning_count, 2U); + EXPECT_GT(health.receive_rate_hz, 0.0); + EXPECT_GT(health.delivery_rate_hz, 0.0); +} + +TEST(ClientRecovery, ValidRecordResetsConsecutiveMalformedCount) { + netft::test::FakeSensor sensor{500.0}; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.receive_timeout = 1s; + std::atomic delivered{}; + netft::Client client{config}; + + for (int count = 0; count < 9; ++count) { + sensor.queue_payload({1, 2}); + } + sensor.queue_record(10, 0x80010000U, 100); + for (int count = 0; count < 9; ++count) { + sensor.queue_payload({1, 2}); + } + sensor.queue_record(13, 0, 104); + client.start([&](const netft::Sample &) { ++delivered; }); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return delivered.load() >= 2; })); + const auto health = client.health(); + EXPECT_FALSE(client.faulted()); + EXPECT_EQ(health.malformed_count, 18U); + EXPECT_EQ(health.lost_count, 2U); + EXPECT_EQ(health.warning_count, 1U); + client.stop(); +} + +TEST(ClientRecovery, TenConsecutiveMalformedRecordsLatchMalformedStorm) { + netft::test::FakeSensor sensor{500.0}; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.receive_timeout = 1s; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + for (int count = 0; count < 10; ++count) { + sensor.queue_payload({1, 2}); + } + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::MalformedStorm); + const auto health = client.health(); + EXPECT_EQ(health.malformed_count, 10U); + EXPECT_EQ(health.state, netft::ClientState::Faulted); + client.stop(); +} + +TEST(ClientRecovery, MalformedRecordDoesNotExtendValidRecordDeadline) { + netft::test::FakeSensor sensor{200.0}; + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.receive_timeout = 200ms; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + + sensor.pause(); + const auto last_valid = std::chrono::steady_clock::now(); + std::this_thread::sleep_for(150ms); + sensor.send_payload_now({1, 2}); + ASSERT_TRUE(wait_until([&] { return client.faulted(); }, 500ms)); + const auto elapsed = std::chrono::steady_clock::now() - last_valid; + + EXPECT_EQ(client.fault_code(), netft::FaultCode::Timeout); + EXPECT_EQ(client.health().malformed_count, 1U); + EXPECT_GE(elapsed, 175ms); + EXPECT_LT(elapsed, 300ms); + client.stop(); +} + +TEST(ClientRecovery, SeriousStatusUsesDeliveryPolicyAndLatchesFailStop) { + for (const bool deliver_error : {false, true}) { + netft::test::FakeSensor sensor{100.0}; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.deliver_samples_with_error_status = deliver_error; + std::atomic delivered{}; + netft::Client client{config}; + client.start([&](const netft::Sample &) { ++delivered; }); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + sensor.queue_record(1, 0x80020000U, 100); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::SeriousStatus); + EXPECT_EQ(delivered.load(), deliver_error ? 1U : 0U); + EXPECT_EQ(client.health().device_error_count, 1U); + client.stop(); + } +} + +TEST(ClientRecovery, FailStopMapsFtStallAndBackwardFaults) { + for (const auto test_case : { + std::pair{100U, netft::FaultCode::FtStall}, + std::pair{90U, netft::FaultCode::FtBackward}, + }) { + netft::test::FakeSensor sensor{200.0}; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + sensor.queue_record(1, 0, 100); + sensor.queue_record(2, 0, test_case.first); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), test_case.second); + EXPECT_EQ(client.health().reconnect_count, 0U); + client.stop(); + } +} + +TEST(ClientRecovery, ReconnectPolicyDropsFtDiscontinuitiesAndConfirmsRestart) { + netft::test::FakeSensor sensor{500.0}; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 1s; + std::mutex delivered_mutex; + std::vector delivered; + netft::Client client{config}; + client.start([&](const netft::Sample &sample) { + std::lock_guard lock(delivered_mutex); + delivered.push_back(sample.ft_sequence); + if (sample.ft_sequence == 6) { + sensor.pause(); + } + }); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + sensor.queue_record(1, 0, 0x00100000U); + sensor.queue_record(2, 0, 0x00100000U); + sensor.queue_record(3, 0, 2); + sensor.queue_record(4, 0, 6); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.health().ft_restart_count == 1; })); + client.stop(); + const auto health = client.health(); + std::lock_guard lock(delivered_mutex); + EXPECT_EQ(delivered, (std::vector{0x00100000U, 6U})); + EXPECT_EQ(health.ft_stall_count, 1U); + EXPECT_EQ(health.ft_backward_count, 1U); + EXPECT_EQ(health.ft_restart_count, 1U); + EXPECT_EQ(health.last_ft_progress, "restart"); + EXPECT_EQ(health.reconnect_count, 0U); +} + +TEST(ClientRecovery, ReconnectResetsRdtOrderingForNewLowSequenceStream) { + netft::test::FakeSensor sensor{500.0}; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 80ms; + std::mutex delivered_mutex; + std::vector delivered; + netft::Client client{config}; + client.start([&](const netft::Sample &sample) { + std::lock_guard lock(delivered_mutex); + delivered.push_back(sample.rdt_sequence); + }); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + sensor.send_record_now(100, 0, 0x00100000U); + ASSERT_TRUE(wait_until([&] { return client.health().delivered_count == 1; })); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 2, 500ms)); + sensor.send_record_now(1, 0, 0x00100004U); + ASSERT_TRUE(wait_until([&] { return client.health().delivered_count == 2; })); + + client.stop(); + const auto health = client.health(); + const auto latest = client.latest_sample(); + std::lock_guard lock(delivered_mutex); + EXPECT_EQ(delivered, (std::vector{100U, 1U})); + ASSERT_TRUE(latest); + EXPECT_EQ(latest->rdt_sequence, 1U); + EXPECT_EQ(latest->configuration_revision, 1U); + EXPECT_EQ(health.received_count, 2U); + EXPECT_EQ(health.delivered_count, 2U); + EXPECT_EQ(health.lost_count, 0U); + EXPECT_EQ(health.duplicate_count, 0U); + EXPECT_EQ(health.out_of_order_count, 0U); + EXPECT_EQ(health.timeout_count, 1U); + EXPECT_EQ(health.reconnect_count, 1U); + ASSERT_TRUE(health.sensor_configuration); + EXPECT_EQ(health.sensor_configuration->revision, 1U); + EXPECT_EQ(sensor.http_request_count(), 2U); +} + +TEST(ClientRecovery, ReconnectPreservesFtHistoryButClearsUnconfirmedRestartCandidate) { + netft::test::FakeSensor sensor{500.0}; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 80ms; + std::mutex delivered_mutex; + std::vector delivered; + netft::Client client{config}; + client.start([&](const netft::Sample &sample) { + std::lock_guard lock(delivered_mutex); + delivered.push_back(sample.ft_sequence); + }); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + sensor.send_record_now(100, 0, 0x00100000U); + ASSERT_TRUE(wait_until([&] { return client.health().delivered_count == 1; })); + sensor.send_record_now(101, 0, 2); + ASSERT_TRUE(wait_until([&] { return client.health().received_count == 2; })); + { + const auto health = client.health(); + EXPECT_EQ(health.delivered_count, 1U); + EXPECT_EQ(health.ft_backward_count, 1U); + EXPECT_EQ(health.ft_restart_count, 0U); + } + + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 2, 500ms)); + sensor.send_record_now(1, 0, 6); + ASSERT_TRUE(wait_until([&] { return client.health().received_count == 3; })); + { + const auto health = client.health(); + EXPECT_EQ(health.delivered_count, 1U); + EXPECT_EQ(health.ft_backward_count, 2U); + EXPECT_EQ(health.ft_restart_count, 0U); + EXPECT_EQ(health.out_of_order_count, 0U); + } + + sensor.send_record_now(2, 0, 10); + ASSERT_TRUE(wait_until([&] { return client.health().ft_restart_count == 1; })); + client.stop(); + + const auto health = client.health(); + const auto latest = client.latest_sample(); + std::lock_guard lock(delivered_mutex); + EXPECT_EQ(delivered, (std::vector{0x00100000U, 10U})); + ASSERT_TRUE(latest); + EXPECT_EQ(latest->rdt_sequence, 2U); + EXPECT_EQ(latest->ft_sequence, 10U); + EXPECT_EQ(latest->configuration_revision, 1U); + EXPECT_EQ(health.received_count, 4U); + EXPECT_EQ(health.delivered_count, 2U); + EXPECT_EQ(health.lost_count, 0U); + EXPECT_EQ(health.duplicate_count, 0U); + EXPECT_EQ(health.out_of_order_count, 0U); + EXPECT_EQ(health.ft_stall_count, 0U); + EXPECT_EQ(health.ft_backward_count, 2U); + EXPECT_EQ(health.ft_restart_count, 1U); + EXPECT_EQ(health.last_ft_progress, "restart"); + EXPECT_EQ(health.timeout_count, 1U); + EXPECT_EQ(health.reconnect_count, 1U); + EXPECT_EQ(health.calibration_change_count, 0U); + ASSERT_TRUE(health.sensor_configuration); + EXPECT_EQ(health.sensor_configuration->revision, 1U); + EXPECT_EQ(sensor.http_request_count(), 2U); +} + +TEST(ClientRecovery, SeriousStatusWinsWhenFtAlsoDiscontinuous) { + netft::test::FakeSensor sensor{200.0}; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + sensor.queue_record(1, 0, 100); + sensor.queue_record(2, 0x80020000U, 100); + sensor.resume(); + + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::SeriousStatus); + EXPECT_EQ(client.health().ft_stall_count, 1U); + EXPECT_EQ(client.health().reconnect_count, 0U); + client.stop(); +} + +TEST(ClientRecovery, FailStopMapsTimeoutSocketAndConfigurationFailures) { + { + netft::test::FakeSensor sensor{200.0}; + sensor.pause(); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + config.receive_timeout = 50ms; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::Timeout); + EXPECT_EQ(client.health().timeout_count, 1U); + client.stop(); + } + { + netft::Config config; + config.sensor_host = "not-a-loopback-host.invalid"; + config.calibration_override = + netft::Calibration{1.0, 1.0, netft::ForceUnit::Newton, netft::TorqueUnit::NewtonMeter}; + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(wait_until([&] { return client.faulted(); }, 2s)); + EXPECT_EQ(client.fault_code(), netft::FaultCode::Socket); + client.stop(); + } + { + netft::test::FakeSensor sensor; + sensor.set_xml_configuration(""); + auto config = config_for(sensor); + config.recovery_policy = netft::RecoveryPolicy::FailStop; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(wait_until([&] { return client.faulted(); })); + EXPECT_EQ(client.fault_code(), netft::FaultCode::SensorConfiguration); + EXPECT_EQ(start_count(sensor), 0U); + client.stop(); + } +} + +TEST(ClientRecovery, ReconnectBackoffDoublesCapsAndResetsAfterValidRecord) { + netft::test::FakeSensor sensor{500.0}; + auto config = config_for(sensor); + config.receive_timeout = 40ms; + config.reconnect_initial_delay = 20ms; + config.reconnect_max_delay = 80ms; + std::atomic delivered{}; + netft::Client client{config}; + client.start([&](const netft::Sample &) { ++delivered; }); + ASSERT_TRUE(wait_until([&] { return delivered.load() > 0; })); + + sensor.pause(); + ASSERT_TRUE(wait_until([&] { return start_times(sensor).size() >= 5; }, 1s)); + const auto starts = start_times(sensor); + ASSERT_GE(starts.size(), 5U); + const auto interval = [&](const std::size_t index) { + return std::chrono::duration_cast(starts[index] - starts[index - 1]); + }; + EXPECT_GE(interval(1), 50ms); + EXPECT_LT(interval(1), 90ms); + EXPECT_GE(interval(2), 70ms); + EXPECT_LT(interval(2), 110ms); + EXPECT_GE(interval(3), 105ms); + EXPECT_LT(interval(3), 150ms); + EXPECT_GE(interval(4), 105ms); + EXPECT_LT(interval(4), 150ms); + + const auto recovery_baseline = delivered.load(); + sensor.resume(); + ASSERT_TRUE(wait_until([&] { return delivered.load() > recovery_baseline; }, 500ms)); + sensor.pause(); + const auto paused_at = std::chrono::steady_clock::now(); + const auto prior_starts = start_times(sensor).size(); + ASSERT_TRUE(wait_until([&] { return start_times(sensor).size() > prior_starts; }, 500ms)); + const auto reset_start = start_times(sensor).back(); + EXPECT_GE(reset_start - paused_at, 45ms); + EXPECT_LT(reset_start - paused_at, 95ms); + EXPECT_FALSE(client.faulted()); + client.stop(); +} + +TEST(ClientRecovery, StopInterruptsReconnectBackoff) { + netft::test::FakeSensor sensor{100.0}; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 40ms; + config.reconnect_initial_delay = 2s; + config.reconnect_max_delay = 2s; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE( + wait_until([&] { return client.health().state == netft::ClientState::Backoff; }, 500ms)); + + const auto stop_started = std::chrono::steady_clock::now(); + client.stop(); + const auto stop_elapsed = std::chrono::steady_clock::now() - stop_started; + + EXPECT_LT(stop_elapsed, 500ms); + EXPECT_EQ(client.health().state, netft::ClientState::Stopped); +} + +TEST(ClientRecovery, ReconnectRediscoversChangedCalibrationAndIncrementsRevision) { + netft::test::FakeSensor sensor{200.0}; + auto config = config_for(sensor); + config.receive_timeout = 60ms; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + ASSERT_EQ(client.latest_sample()->configuration_revision, 1U); + + sensor.set_xml_configuration(kChangedConfiguration); + sensor.pause(); + ASSERT_TRUE(sensor.wait_for_http_request(2, 500ms)); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 2, 500ms)); + sensor.resume(); + ASSERT_TRUE(wait_until([&] { + const auto sample = client.latest_sample(); + return sample && sample->configuration_revision == 2; + })); + + const auto sample = client.latest_sample(); + ASSERT_TRUE(sample); + EXPECT_EQ(sample->torque_unit, netft::TorqueUnit::NewtonMillimeter); + EXPECT_EQ(sample->configuration_revision, 2U); + const auto health = client.health(); + ASSERT_TRUE(health.sensor_configuration); + EXPECT_EQ(health.sensor_configuration->revision, 2U); + EXPECT_EQ(health.calibration_change_count, 1U); + client.stop(); +} + +TEST(ClientRecovery, InvalidReplacementConfigurationCannotStartNewRdtSession) { + for (const std::string replacement : { + "", + R"xml(Fake1000000 +1000Nmystery)xml", + }) { + netft::test::FakeSensor sensor{200.0}; + auto config = config_for(sensor); + config.receive_timeout = 60ms; + config.reconnect_initial_delay = 10ms; + config.reconnect_max_delay = 10ms; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + ASSERT_EQ(start_count(sensor), 1U); + + sensor.set_xml_configuration(replacement); + sensor.pause(); + ASSERT_TRUE(sensor.wait_for_http_request(3, 500ms)); + EXPECT_EQ(start_count(sensor), 1U); + EXPECT_EQ(client.latest_sample()->configuration_revision, 1U); + EXPECT_EQ(client.health().calibration_change_count, 0U); + EXPECT_FALSE(client.faulted()); + client.stop(); + } +} + +} // namespace diff --git a/test/core/test_client_stream.cpp b/test/core/test_client_stream.cpp new file mode 100644 index 0000000..c268880 --- /dev/null +++ b/test/core/test_client_stream.cpp @@ -0,0 +1,240 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "detail/protocol.hpp" +#include "netft/client.hpp" +#include "support/fake_sensor.hpp" + +namespace { + +using namespace std::chrono_literals; + +netft::Config config_for(const netft::test::FakeSensor &sensor) { + netft::Config config; + config.sensor_host = sensor.host(); + config.rdt_port = sensor.rdt_port(); + config.http_port = sensor.http_port(); + config.receive_timeout = 200ms; + config.configuration_connect_timeout = 200ms; + config.configuration_timeout = 500ms; + return config; +} + +template +bool wait_until(Predicate predicate, const std::chrono::milliseconds timeout = 1s) { + const auto deadline = std::chrono::steady_clock::now() + timeout; + do { + if (predicate()) { + return true; + } + std::this_thread::yield(); + } while (std::chrono::steady_clock::now() < deadline); + return predicate(); +} + +class TwoPartyBarrier { +public: + void arrive_and_wait() { + std::unique_lock lock(mutex_); + const auto generation = generation_; + if (++arrivals_ == 2) { + arrivals_ = 0; + ++generation_; + cv_.notify_one(); + return; + } + cv_.wait(lock, [this, generation] { return generation_ != generation; }); + } + +private: + std::mutex mutex_; + std::condition_variable cv_; + unsigned arrivals_{}; + unsigned generation_{}; +}; + +TEST(ClientStream, ConstructionPerformsNoNetworkIo) { + netft::test::FakeSensor sensor; + + netft::Client client{config_for(sensor)}; + + EXPECT_EQ(sensor.http_request_count(), 0U); + EXPECT_TRUE(sensor.commands().empty()); + EXPECT_EQ(client.health().state, netft::ClientState::Stopped); + EXPECT_FALSE(client.faulted()); + EXPECT_EQ(client.fault_code(), netft::FaultCode::None); + EXPECT_FALSE(client.latest_sample()); + EXPECT_FALSE(client.wait_for_first_sample(1ms)); + EXPECT_THROW(client.bias(), netft::NotConnectedError); +} + +TEST(ClientStream, DiscoveryCompletesBeforeRealtimeStarts) { + netft::test::FakeSensor sensor; + sensor.pause(); + sensor.set_http_response_delay(100ms); + netft::Client client{config_for(sensor)}; + + client.start([](const netft::Sample &) {}); + + ASSERT_TRUE(sensor.wait_for_http_request()); + EXPECT_FALSE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 1, 20ms)); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime, 1, 500ms)); + EXPECT_EQ(sensor.http_request_count(), 1U); + client.stop(); +} + +TEST(ClientStream, ConvertsNativeUnitsAndPublishesLatestSample) { + netft::test::FakeSensor sensor; + sensor.pause(); + sensor.queue_record(1, 0, 100, + {1'000'000, -2'000'000, 3'000'000, 1'000'000, -2'000'000, 3'000'000}); + netft::Client client{config_for(sensor)}; + std::mutex callback_mutex; + std::optional callback_sample; + client.start([&](const netft::Sample &sample) { + std::lock_guard lock(callback_mutex); + if (!callback_sample) { + callback_sample = sample; + sensor.pause(); + } + }); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + sensor.resume(); + + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + const auto sample = client.latest_sample(); + ASSERT_TRUE(sample); + EXPECT_EQ(sample->rdt_sequence, 1U); + EXPECT_EQ(sample->ft_sequence, 100U); + EXPECT_EQ(sample->force, (std::array{1.0, -2.0, 3.0})); + EXPECT_EQ(sample->torque, (std::array{1.0, -2.0, 3.0})); + EXPECT_EQ(sample->force_unit, netft::ForceUnit::Newton); + EXPECT_EQ(sample->torque_unit, netft::TorqueUnit::NewtonMeter); + EXPECT_EQ(sample->configuration_revision, 1U); + { + std::lock_guard lock(callback_mutex); + ASSERT_TRUE(callback_sample); + EXPECT_EQ(callback_sample->rdt_sequence, sample->rdt_sequence); + EXPECT_EQ(callback_sample->force, sample->force); + EXPECT_EQ(callback_sample->torque, sample->torque); + } + + const auto health = client.health(); + ASSERT_TRUE(health.sensor_configuration); + EXPECT_EQ(health.sensor_configuration->source, netft::CalibrationSource::Sensor); + EXPECT_EQ(health.sensor_configuration->revision, 1U); + EXPECT_EQ(health.received_count, 1U); + EXPECT_EQ(health.delivered_count, 1U); + client.stop(); +} + +TEST(ClientStream, CompleteCalibrationOverrideSkipsHttpDiscovery) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.calibration_override = netft::Calibration{100.0, 10.0, netft::ForceUnit::PoundForce, + netft::TorqueUnit::PoundForceFoot}; + netft::Client client{config}; + + client.start([](const netft::Sample &) {}); + + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + EXPECT_EQ(sensor.http_request_count(), 0U); + const auto health = client.health(); + ASSERT_TRUE(health.sensor_configuration); + EXPECT_EQ(health.sensor_configuration->source, netft::CalibrationSource::Override); + EXPECT_EQ(health.sensor_configuration->calibration.force_unit, netft::ForceUnit::PoundForce); + client.stop(); +} + +TEST(ClientStream, StopIsIdempotentAndSendsOneEffectiveCommand) { + netft::test::FakeSensor sensor; + netft::Client client{config_for(sensor)}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + client.stop(); + client.stop(); + + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StopStreaming)); + const auto commands = sensor.commands(); + EXPECT_EQ(std::count(commands.begin(), commands.end(), netft::detail::Command::StopStreaming), 1); + EXPECT_EQ(client.health().state, netft::ClientState::Stopped); +} + +TEST(ClientStream, StopWakesBlockedReceiveBeforeLongTimeout) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 10s; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + + std::this_thread::sleep_for(20ms); + const auto before_stop = std::chrono::steady_clock::now(); + client.stop(); + const auto elapsed = std::chrono::steady_clock::now() - before_stop; + + EXPECT_LT(elapsed, 500ms); + EXPECT_EQ(client.health().state, netft::ClientState::Stopped); +} + +TEST(ClientStream, ConcurrentBiasAndRecordTrackingRemainConsistent) { + netft::test::FakeSensor sensor; + sensor.pause(); + auto config = config_for(sensor); + config.receive_timeout = 2s; + netft::Client client{config}; + client.start([](const netft::Sample &) {}); + ASSERT_TRUE(sensor.wait_for_command(netft::detail::Command::StartRealtime)); + sensor.send_record_now(0, 0, 996); + ASSERT_TRUE(client.wait_for_first_sample(500ms)); + + constexpr std::uint32_t kIterations = 200; + TwoPartyBarrier barrier; + std::exception_ptr bias_error; + std::thread bias_thread([&] { + try { + for (std::uint32_t iteration = 0; iteration < kIterations; ++iteration) { + barrier.arrive_and_wait(); + client.bias(); + barrier.arrive_and_wait(); + } + } catch (...) { + bias_error = std::current_exception(); + } + }); + + bool all_records_received = true; + for (std::uint32_t iteration = 0; iteration < kIterations; ++iteration) { + barrier.arrive_and_wait(); + sensor.send_record_now(iteration + 1, 0, 1000 + iteration * 4); + barrier.arrive_and_wait(); + all_records_received = wait_until([&client, iteration] { + return client.health().received_count >= iteration + 2; + }) && + all_records_received; + } + bias_thread.join(); + + ASSERT_EQ(bias_error, nullptr); + EXPECT_TRUE(all_records_received); + const auto health = client.health(); + EXPECT_EQ(health.received_count, kIterations + 1); + EXPECT_EQ(health.lost_count, 0U); + EXPECT_EQ(health.duplicate_count, 0U); + EXPECT_EQ(health.out_of_order_count, 0U); + client.stop(); +} + +} // namespace diff --git a/test/core/test_discovery.cpp b/test/core/test_discovery.cpp new file mode 100644 index 0000000..ce40f58 --- /dev/null +++ b/test/core/test_discovery.cpp @@ -0,0 +1,442 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "detail/xml_config.hpp" +#include "netft/discovery.hpp" +#include "support/fake_http_server.hpp" + +namespace { + +constexpr std::string_view kValidXml = R"xml( +Ethernet Axia1000000 +1000000NNm)xml"; + +std::string replace_value(std::string_view xml, std::string_view tag, std::string_view value) { + std::string result{xml}; + const std::string opening = "<" + std::string{tag} + ">"; + const std::string closing = ""; + const auto begin = result.find(opening) + opening.size(); + const auto end = result.find(closing, begin); + result.replace(begin, end - begin, value); + return result; +} + +std::string remove_element(std::string_view xml, std::string_view tag) { + std::string result{xml}; + const std::string opening = "<" + std::string{tag} + ">"; + const std::string closing = ""; + const auto begin = result.find(opening); + const auto end = result.find(closing, begin) + closing.size(); + result.erase(begin, end - begin); + return result; +} + +std::string duplicate_element(std::string_view xml, std::string_view tag) { + std::string result{xml}; + const std::string opening = "<" + std::string{tag} + ">"; + const std::string closing = ""; + const auto begin = result.find(opening); + const auto end = result.find(closing, begin) + closing.size(); + result.insert(end, result.substr(begin, end - begin)); + return result; +} + +std::string insert_after_root_open(std::string_view xml, std::string_view markup) { + std::string result{xml}; + const auto position = result.find("") + std::string_view{""}.size(); + result.insert(position, markup); + return result; +} + +netft::DiscoveryOptions options_for(const FakeHttpServer &server) { + netft::DiscoveryOptions options; + options.sensor_host = server.host(); + options.http_port = server.port(); + options.connect_timeout = std::chrono::milliseconds{200}; + options.total_timeout = std::chrono::milliseconds{500}; + return options; +} + +TEST(XmlConfiguration, ParsesTheRealSensorFixtureExactly) { + const auto result = netft::detail::parse_sensor_configuration(kValidXml); + + EXPECT_EQ(result.product_name, "Ethernet Axia"); + EXPECT_DOUBLE_EQ(result.calibration.counts_per_force_unit, 1'000'000.0); + EXPECT_DOUBLE_EQ(result.calibration.counts_per_torque_unit, 1'000'000.0); + EXPECT_EQ(result.calibration.force_unit, netft::ForceUnit::Newton); + EXPECT_EQ(result.calibration.torque_unit, netft::TorqueUnit::NewtonMeter); + EXPECT_EQ(result.source, netft::CalibrationSource::Sensor); + EXPECT_EQ(result.revision, 1U); +} + +class RequiredXmlField : public ::testing::TestWithParam {}; + +TEST_P(RequiredXmlField, RejectsMissingFields) { + EXPECT_THROW(netft::detail::parse_sensor_configuration(remove_element(kValidXml, GetParam())), + netft::DiscoveryError); +} + +TEST_P(RequiredXmlField, RejectsDuplicateFields) { + EXPECT_THROW(netft::detail::parse_sensor_configuration(duplicate_element(kValidXml, GetParam())), + netft::DiscoveryError); +} + +TEST_P(RequiredXmlField, RejectsEmptyFields) { + EXPECT_THROW( + netft::detail::parse_sensor_configuration(replace_value(kValidXml, GetParam(), " \t\r\n")), + netft::DiscoveryError); +} + +TEST_P(RequiredXmlField, RejectsFieldsLongerThan128Bytes) { + EXPECT_THROW(netft::detail::parse_sensor_configuration( + replace_value(kValidXml, GetParam(), std::string(129, '1'))), + netft::DiscoveryError); +} + +INSTANTIATE_TEST_SUITE_P(EveryField, RequiredXmlField, + ::testing::Values("prodname", "cfgcpf", "cfgcpt", "scfgfu", "scfgtu")); + +struct InvalidCountCase { + const char *tag; + const char *value; +}; + +class InvalidXmlCount : public ::testing::TestWithParam {}; + +TEST_P(InvalidXmlCount, RejectsInvalidCounts) { + const auto ¶meter = GetParam(); + EXPECT_THROW(netft::detail::parse_sensor_configuration( + replace_value(kValidXml, parameter.tag, parameter.value)), + netft::DiscoveryError); +} + +INSTANTIATE_TEST_SUITE_P( + EveryInvalidForm, InvalidXmlCount, + ::testing::Values(InvalidCountCase{"cfgcpf", "not-a-number"}, + InvalidCountCase{"cfgcpf", "10remaining"}, InvalidCountCase{"cfgcpf", "+1"}, + InvalidCountCase{"cfgcpf", "0x1p2"}, InvalidCountCase{"cfgcpf", "1e"}, + InvalidCountCase{"cfgcpf", "0"}, InvalidCountCase{"cfgcpf", "-1"}, + InvalidCountCase{"cfgcpf", "NaN"}, InvalidCountCase{"cfgcpf", "inf"}, + InvalidCountCase{"cfgcpt", "not-a-number"}, + InvalidCountCase{"cfgcpt", "10remaining"}, InvalidCountCase{"cfgcpt", "0"}, + InvalidCountCase{"cfgcpt", "-1"}, InvalidCountCase{"cfgcpt", "NaN"}, + InvalidCountCase{"cfgcpt", "infinity"})); + +TEST(XmlConfiguration, AcceptsGeneralDecimalCountForms) { + constexpr std::array, 4> cases{{ + {".5", 0.5}, + {"1.", 1.0}, + {"1e+2", 100.0}, + {"2.5E-1", 0.25}, + }}; + + for (const auto &[spelling, expected] : cases) { + SCOPED_TRACE(spelling); + const auto result = + netft::detail::parse_sensor_configuration(replace_value(kValidXml, "cfgcpf", spelling)); + EXPECT_DOUBLE_EQ(result.calibration.counts_per_force_unit, expected); + } +} + +class CommaDecimalPoint final : public std::numpunct { + protected: + char do_decimal_point() const override { return ','; } +}; + +TEST(XmlConfiguration, ParsesCountsIndependentlyOfTheGlobalLocale) { + const auto original_locale = std::locale(); + std::locale::global(std::locale{std::locale::classic(), new CommaDecimalPoint}); + try { + const auto result = + netft::detail::parse_sensor_configuration(replace_value(kValidXml, "cfgcpf", "1.25")); + EXPECT_DOUBLE_EQ(result.calibration.counts_per_force_unit, 1.25); + EXPECT_THROW( + netft::detail::parse_sensor_configuration(replace_value(kValidXml, "cfgcpf", "1,25")), + netft::DiscoveryError); + } catch (...) { + std::locale::global(original_locale); + throw; + } + std::locale::global(original_locale); +} + +TEST(XmlConfiguration, TrimsAsciiWhitespaceAroundEveryValue) { + auto xml = replace_value(kValidXml, "prodname", " \tEthernet Axia\r\n"); + xml = replace_value(xml, "cfgcpf", "\n 1000000 \t"); + xml = replace_value(xml, "cfgcpt", "\r1000000 "); + xml = replace_value(xml, "scfgfu", " N\n"); + xml = replace_value(xml, "scfgtu", "\tNm "); + + const auto result = netft::detail::parse_sensor_configuration(xml); + EXPECT_EQ(result.product_name, "Ethernet Axia"); + EXPECT_DOUBLE_EQ(result.calibration.counts_per_force_unit, 1'000'000.0); + EXPECT_DOUBLE_EQ(result.calibration.counts_per_torque_unit, 1'000'000.0); + EXPECT_EQ(result.calibration.force_unit, netft::ForceUnit::Newton); + EXPECT_EQ(result.calibration.torque_unit, netft::TorqueUnit::NewtonMeter); +} + +TEST(XmlConfiguration, AcceptsEveryApprovedForceUnitSpelling) { + constexpr std::array, 5> cases{{ + {"lbf", netft::ForceUnit::PoundForce}, + {"N", netft::ForceUnit::Newton}, + {"klbf", netft::ForceUnit::KiloPoundForce}, + {"kN", netft::ForceUnit::KiloNewton}, + {"kgf", netft::ForceUnit::KilogramForce}, + }}; + + for (const auto &[spelling, expected] : cases) { + SCOPED_TRACE(spelling); + const auto result = + netft::detail::parse_sensor_configuration(replace_value(kValidXml, "scfgfu", spelling)); + EXPECT_EQ(result.calibration.force_unit, expected); + } +} + +TEST(XmlConfiguration, AcceptsEveryApprovedAtiTorqueUnitSpelling) { + constexpr std::array, 8> cases{{ + {"Nm", netft::TorqueUnit::NewtonMeter}, + {"N-m", netft::TorqueUnit::NewtonMeter}, + {"Nmm", netft::TorqueUnit::NewtonMillimeter}, + {"N-mm", netft::TorqueUnit::NewtonMillimeter}, + {"lbf-in", netft::TorqueUnit::PoundForceInch}, + {"lbf-ft", netft::TorqueUnit::PoundForceFoot}, + {"kg-cm", netft::TorqueUnit::KilogramForceCentimeter}, + {"kN-m", netft::TorqueUnit::KiloNewtonMeter}, + }}; + + for (const auto &[spelling, expected] : cases) { + SCOPED_TRACE(spelling); + const auto result = + netft::detail::parse_sensor_configuration(replace_value(kValidXml, "scfgtu", spelling)); + EXPECT_EQ(result.calibration.torque_unit, expected); + } +} + +TEST(XmlConfiguration, RejectsUnknownUnits) { + EXPECT_THROW( + netft::detail::parse_sensor_configuration(replace_value(kValidXml, "scfgfu", "newtons")), + netft::DiscoveryError); + EXPECT_THROW(netft::detail::parse_sensor_configuration( + replace_value(kValidXml, "scfgtu", "newton-metres")), + netft::DiscoveryError); +} + +TEST(XmlConfiguration, IgnoresRequiredElementTextInsideComments) { + const auto xml = insert_after_root_open(kValidXml, ""); + + const auto result = netft::detail::parse_sensor_configuration(xml); + + EXPECT_EQ(result.product_name, "Ethernet Axia"); +} + +TEST(XmlConfiguration, RejectsAFieldThatAppearsOnlyInsideAComment) { + const auto xml = insert_after_root_open(remove_element(kValidXml, "prodname"), + ""); + + EXPECT_THROW(netft::detail::parse_sensor_configuration(xml), netft::DiscoveryError); +} + +TEST(XmlConfiguration, IgnoresRequiredElementTextInsideCdata) { + const auto xml = + insert_after_root_open(kValidXml, "Fake Sensor]]>"); + + const auto result = netft::detail::parse_sensor_configuration(xml); + + EXPECT_EQ(result.product_name, "Ethernet Axia"); +} + +TEST(XmlConfiguration, RejectsAFieldThatAppearsOnlyInsideCdata) { + const auto xml = insert_after_root_open(remove_element(kValidXml, "prodname"), + "Fake Sensor]]>"); + + EXPECT_THROW(netft::detail::parse_sensor_configuration(xml), netft::DiscoveryError); +} + +TEST(XmlConfiguration, RejectsUnterminatedCommentsAndCdata) { + EXPECT_THROW( + netft::detail::parse_sensor_configuration(std::string{kValidXml} + "