From 484018c8277c3e2ce0ad0d25b8df223b302d913f Mon Sep 17 00:00:00 2001 From: flipflip Date: Tue, 18 Aug 2026 16:34:03 +0200 Subject: [PATCH] refactor ROS1 BagWriter to fpsdk_common, add necessary "rosnoros" deps, move fpsdk_ros[12] stuff to fpsdk_common/ros[12], remove fpsdk_ros[12] and examples --- .devcontainer/.bashrc | 3 + .devcontainer/humble/devcontainer.json | 2 +- .devcontainer/jazzy/devcontainer.json | 2 +- .devcontainer/lyrical/devcontainer.json | 2 +- .devcontainer/noetic/devcontainer.json | 2 +- .gitignore | 1 + CMakeLists.txt | 6 - Makefile | 28 +- README.md | 3 +- docker/ci.sh | 21 +- docker/scripts/install_apt_base.sh | 1 + examples/camera_streaming/Makefile | 2 - examples/fpb_measurements/Makefile | 2 - examples/fusion_epoch/Makefile | 2 - examples/parser_intro/Makefile | 2 - examples/ros1_fpsdk_demo/CMakeLists.txt | 97 --- examples/ros1_fpsdk_demo/LICENSE | 21 - examples/ros1_fpsdk_demo/README.md | 40 - .../include/ros1_fpsdk_demo/node.hpp | 69 -- examples/ros1_fpsdk_demo/launch/config.yaml | 4 - examples/ros1_fpsdk_demo/launch/console.conf | 2 - examples/ros1_fpsdk_demo/launch/node.launch | 8 - examples/ros1_fpsdk_demo/package.xml | 13 - examples/ros1_fpsdk_demo/src/node.cpp | 188 ----- examples/ros1_fpsdk_demo/src/node_main.cpp | 111 --- examples/ros2_fpsdk_demo/CMakeLists.txt | 87 --- examples/ros2_fpsdk_demo/LICENSE | 21 - examples/ros2_fpsdk_demo/README.md | 38 - .../include/ros2_fpsdk_demo/node.hpp | 72 -- examples/ros2_fpsdk_demo/launch/config.yaml | 6 - examples/ros2_fpsdk_demo/launch/node.launch | 10 - examples/ros2_fpsdk_demo/package.xml | 21 - examples/ros2_fpsdk_demo/src/node.cpp | 210 ----- examples/ros2_fpsdk_demo/src/node_main.cpp | 120 --- fpsdk.code-workspace | 4 +- fpsdk_apps/CMakeLists.txt | 43 +- fpsdk_apps/fpltool/fpltool_dump.cpp | 2 +- fpsdk_apps/fpltool/fpltool_extract.cpp | 88 +-- fpsdk_apps/fpltool/fpltool_extract.hpp | 15 +- fpsdk_apps/fpltool/fpltool_meta.cpp | 2 +- fpsdk_apps/fpltool/fpltool_opts.hpp | 45 +- fpsdk_apps/fpltool/fpltool_trim.cpp | 2 +- fpsdk_apps/package.xml | 4 - fpsdk_common/CMakeLists.txt | 59 +- fpsdk_common/LICENSE | 3 +- fpsdk_common/cmake/setup.cmake | 4 +- fpsdk_common/cmake/utils.cmake | 32 + fpsdk_common/doc/doc.hpp | 1 + fpsdk_common/include/fpsdk_common/logging.hpp | 2 +- fpsdk_common/include/fpsdk_common/parser.hpp | 2 +- fpsdk_common/include/fpsdk_common/ros1.hpp | 172 +++- .../include/fpsdk_common/ros2.hpp | 184 ++++- fpsdk_common/include/fpsdk_common/trafo.hpp | 4 +- fpsdk_common/include/fpsdk_common/video.hpp | 6 +- fpsdk_common/rosnoros/ros/time.cpp | 331 ++++++++ fpsdk_common/rosnoros/ros/time.h | 226 +++++- fpsdk_common/src/app.cpp | 28 +- fpsdk_common/src/ros1.cpp | 614 +++++++++++++++ .../src/ros2.cpp | 81 +- fpsdk_common/test/ros1_test.cpp | 738 +++++++++++++++++- .../test/ros2_test.cpp | 17 +- fpsdk_doc/Doxyfile | 12 +- fpsdk_doc/fpsdk-overview.drawio.svg | 731 +++++++---------- fpsdk_doc/fpsdk_build.hpp | 119 +-- fpsdk_doc/fpsdk_doc.hpp | 6 +- fpsdk_doc/fpsdk_examples.hpp | 13 - fpsdk_ros1/CMakeLists.txt | 165 ---- fpsdk_ros1/LICENSE | 21 - fpsdk_ros1/README.md | 5 - fpsdk_ros1/cmake/config.cmake.in | 19 - fpsdk_ros1/cmake/config.pc.in | 12 - fpsdk_ros1/doc/doc.hpp | 47 -- fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp | 139 ---- .../include/fpsdk_ros1/ext/cv_bridge.hpp | 10 - .../fpsdk_ros1/ext/eigen_conversions.hpp | 20 - fpsdk_ros1/include/fpsdk_ros1/ext/ros.hpp | 10 - .../fpsdk_ros1/ext/ros_callback_queue.hpp | 10 - .../include/fpsdk_ros1/ext/ros_console.hpp | 10 - .../include/fpsdk_ros1/ext/ros_msgs.hpp | 11 - .../include/fpsdk_ros1/ext/ros_param.hpp | 10 - .../fpsdk_ros1/ext/ros_serialization.hpp | 10 - .../include/fpsdk_ros1/ext/ros_time.hpp | 10 - .../include/fpsdk_ros1/ext/ros_timer.hpp | 10 - .../include/fpsdk_ros1/ext/rosbag_bag.hpp | 10 - .../include/fpsdk_ros1/ext/rosbag_view.hpp | 10 - .../include/fpsdk_ros1/ext/topic_tools.hpp | 10 - fpsdk_ros1/include/fpsdk_ros1/utils.hpp | 145 ---- fpsdk_ros1/package.xml | 16 - fpsdk_ros1/src/bagwriter.cpp | 135 ---- fpsdk_ros1/src/utils.cpp | 126 --- fpsdk_ros1/test/bagwriter_test.cpp | 46 -- fpsdk_ros1/test/utils_test.cpp | 92 --- fpsdk_ros2/CMakeLists.txt | 169 ---- fpsdk_ros2/LICENSE | 21 - fpsdk_ros2/README.md | 5 - fpsdk_ros2/cmake/config.cmake.in | 19 - fpsdk_ros2/cmake/config.pc.in | 12 - fpsdk_ros2/doc/doc.hpp | 48 -- fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp | 133 ---- fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp | 17 - fpsdk_ros2/include/fpsdk_ros2/ext/rclcpp.hpp | 10 - .../fpsdk_ros2/ext/rosbag2_cpp_writer.hpp | 11 - fpsdk_ros2/include/fpsdk_ros2/utils.hpp | 80 -- fpsdk_ros2/package.xml | 18 - fpsdk_ros2/src/ros1.cpp | 34 - fpsdk_ros2/src/utils.cpp | 77 -- fpsdk_ros2/test/bagwriter_test.cpp | 46 -- fpsdk_ros2/test/ros1_test.cpp | 46 -- 108 files changed, 2911 insertions(+), 3751 deletions(-) delete mode 100644 examples/ros1_fpsdk_demo/CMakeLists.txt delete mode 100644 examples/ros1_fpsdk_demo/LICENSE delete mode 100644 examples/ros1_fpsdk_demo/README.md delete mode 100644 examples/ros1_fpsdk_demo/include/ros1_fpsdk_demo/node.hpp delete mode 100644 examples/ros1_fpsdk_demo/launch/config.yaml delete mode 100644 examples/ros1_fpsdk_demo/launch/console.conf delete mode 100644 examples/ros1_fpsdk_demo/launch/node.launch delete mode 100644 examples/ros1_fpsdk_demo/package.xml delete mode 100644 examples/ros1_fpsdk_demo/src/node.cpp delete mode 100644 examples/ros1_fpsdk_demo/src/node_main.cpp delete mode 100644 examples/ros2_fpsdk_demo/CMakeLists.txt delete mode 100644 examples/ros2_fpsdk_demo/LICENSE delete mode 100644 examples/ros2_fpsdk_demo/README.md delete mode 100644 examples/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp delete mode 100644 examples/ros2_fpsdk_demo/launch/config.yaml delete mode 100644 examples/ros2_fpsdk_demo/launch/node.launch delete mode 100644 examples/ros2_fpsdk_demo/package.xml delete mode 100644 examples/ros2_fpsdk_demo/src/node.cpp delete mode 100644 examples/ros2_fpsdk_demo/src/node_main.cpp rename fpsdk_ros2/include/fpsdk_ros2/ros1.hpp => fpsdk_common/include/fpsdk_common/ros2.hpp (52%) create mode 100644 fpsdk_common/rosnoros/ros/time.cpp rename fpsdk_ros2/src/bagwriter.cpp => fpsdk_common/src/ros2.cpp (59%) rename fpsdk_ros2/test/utils_test.cpp => fpsdk_common/test/ros2_test.cpp (79%) delete mode 100644 fpsdk_ros1/CMakeLists.txt delete mode 100644 fpsdk_ros1/LICENSE delete mode 100644 fpsdk_ros1/README.md delete mode 100644 fpsdk_ros1/cmake/config.cmake.in delete mode 100644 fpsdk_ros1/cmake/config.pc.in delete mode 100644 fpsdk_ros1/doc/doc.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/cv_bridge.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/eigen_conversions.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros_callback_queue.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros_console.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros_param.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros_serialization.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros_time.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/ros_timer.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_bag.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_view.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/ext/topic_tools.hpp delete mode 100644 fpsdk_ros1/include/fpsdk_ros1/utils.hpp delete mode 100644 fpsdk_ros1/package.xml delete mode 100644 fpsdk_ros1/src/bagwriter.cpp delete mode 100644 fpsdk_ros1/src/utils.cpp delete mode 100644 fpsdk_ros1/test/bagwriter_test.cpp delete mode 100644 fpsdk_ros1/test/utils_test.cpp delete mode 100644 fpsdk_ros2/CMakeLists.txt delete mode 100644 fpsdk_ros2/LICENSE delete mode 100644 fpsdk_ros2/README.md delete mode 100644 fpsdk_ros2/cmake/config.cmake.in delete mode 100644 fpsdk_ros2/cmake/config.pc.in delete mode 100644 fpsdk_ros2/doc/doc.hpp delete mode 100644 fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp delete mode 100644 fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp delete mode 100644 fpsdk_ros2/include/fpsdk_ros2/ext/rclcpp.hpp delete mode 100644 fpsdk_ros2/include/fpsdk_ros2/ext/rosbag2_cpp_writer.hpp delete mode 100644 fpsdk_ros2/include/fpsdk_ros2/utils.hpp delete mode 100644 fpsdk_ros2/package.xml delete mode 100644 fpsdk_ros2/src/ros1.cpp delete mode 100644 fpsdk_ros2/src/utils.cpp delete mode 100644 fpsdk_ros2/test/bagwriter_test.cpp delete mode 100644 fpsdk_ros2/test/ros1_test.cpp diff --git a/.devcontainer/.bashrc b/.devcontainer/.bashrc index e602d262..c5aa8b77 100644 --- a/.devcontainer/.bashrc +++ b/.devcontainer/.bashrc @@ -30,6 +30,8 @@ export LESSCHARSET=UTF-8 export EDITOR='vim' export GIT_EDITOR=vim +export PATH="$HOME/.local/bin:$PATH" + # nicely coloured man pages export LESS_TERMCAP_mb=$'\E[01;31m' export LESS_TERMCAP_md=$'\E[01;31m' @@ -171,3 +173,4 @@ if [ -n "${ROS_DISTRO}" ]; then esac fi + diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json index f9d3868d..5babd123 100644 --- a/.devcontainer/humble/devcontainer.json +++ b/.devcontainer/humble/devcontainer.json @@ -3,7 +3,7 @@ "dockerComposeFile": "../../docker/docker-compose.yaml", "shutdownAction": "stopCompose", - "name": "Fixposition SDK (ROS Humble)", "service": "humble-dev", "runServices": [ "humble-dev" ], + "name": "Fixposition SDK (ROS2 Humble)", "service": "humble-dev", "runServices": [ "humble-dev" ], // Use this user in the container, re-map IDs to local user automatically "remoteUser": "fpsdk", diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index 9090e242..c6b22537 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -3,7 +3,7 @@ "dockerComposeFile": "../../docker/docker-compose.yaml", "shutdownAction": "stopCompose", - "name": "Fixposition SDK (ROS Jazzy)", "service": "jazzy-dev", "runServices": [ "jazzy-dev" ], + "name": "Fixposition SDK (ROS2 Jazzy)", "service": "jazzy-dev", "runServices": [ "jazzy-dev" ], // Use this user in the container, re-map IDs to local user automatically "remoteUser": "fpsdk", diff --git a/.devcontainer/lyrical/devcontainer.json b/.devcontainer/lyrical/devcontainer.json index 84f53f3c..c1f529fe 100644 --- a/.devcontainer/lyrical/devcontainer.json +++ b/.devcontainer/lyrical/devcontainer.json @@ -3,7 +3,7 @@ "dockerComposeFile": "../../docker/docker-compose.yaml", "shutdownAction": "stopCompose", - "name": "Fixposition SDK (ROS Lyrical)", "service": "lyrical-dev", "runServices": [ "lyrical-dev" ], + "name": "Fixposition SDK (ROS2 Lyrical)", "service": "lyrical-dev", "runServices": [ "lyrical-dev" ], // Use this user in the container, re-map IDs to local user automatically "remoteUser": "fpsdk", diff --git a/.devcontainer/noetic/devcontainer.json b/.devcontainer/noetic/devcontainer.json index 8700dfea..1722256e 100644 --- a/.devcontainer/noetic/devcontainer.json +++ b/.devcontainer/noetic/devcontainer.json @@ -3,7 +3,7 @@ "dockerComposeFile": "../../docker/docker-compose.yaml", "shutdownAction": "stopCompose", - "name": "Fixposition SDK (ROS Noetic)", "service": "noetic-dev", "runServices": [ "noetic-dev" ], + "name": "Fixposition SDK (ROS1 Noetic)", "service": "noetic-dev", "runServices": [ "noetic-dev" ], // Use this user in the container, re-map IDs to local user automatically "remoteUser": "fpsdk", diff --git a/.gitignore b/.gitignore index dab841b8..30ef670f 100644 --- a/.gitignore +++ b/.gitignore @@ -41,6 +41,7 @@ baz* /*.gz /*.json /.claude +/*_bag # We must not commit such files ever. See section FPSDK_BUILD_ROS in fpsdk_doc/fpsdk_build.hpp CATKIN_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt index b40bcc7c..53967d07 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,10 +8,4 @@ project(fpsdk ) add_subdirectory(fpsdk_common) -if(FPSDK_USE_ROS1) - add_subdirectory(fpsdk_ros1) -endif() -if(FPSDK_USE_ROS2) - add_subdirectory(fpsdk_ros2) -endif() add_subdirectory(fpsdk_apps) diff --git a/Makefile b/Makefile index 0ac1b8cd..a8f658f8 100644 --- a/Makefile +++ b/Makefile @@ -12,31 +12,22 @@ BUILD_TESTING = VERBOSE = 0 USE_PROJ = USE_FFMPEG = +USE_BZ2 = C_COMPILER = CXX_COMPILER = # User vars -include config.mk -# Check if we have ROS -FPSDK_USE_ROS1= -FPSDK_USE_ROS2= -ifneq ($(ROS_PACKAGE_PATH),) - FPSDK_USE_ROS1=yes -else ifeq ($(ROS_VERSION),2) - FPSDK_USE_ROS2=yes -# else -# $(info No ROS_PACKAGE_PATH (ROS1) and no ROS_VERSION (ROS2) found) -endif - # A unique ID for this exact config we're using -configuid=$(shell echo "$(BUILD_TYPE) $(INSTALL_PREFIX) $(BUILD_TESTING) $(USE_PROJ) $(USE_FFMPEG) $(FPSDK_VERSION_STRING) $(C_COMPILER) $(CXX_COMPILER) $$(uname -a)" | md5sum | cut -d " " -f1) +configuid=$(shell echo "$(BUILD_TYPE) $(INSTALL_PREFIX) $(BUILD_TESTING) $(USE_PROJ) $(USE_FFMPEG) $(USE_BZ2) $(FPSDK_VERSION_STRING) $(C_COMPILER) $(CXX_COMPILER) $$(uname -a)" | md5sum | cut -d " " -f1) .PHONY: help help: @echo "Usage:" @echo - @echo " make [INSTALL_PREFIX=...] [BUILD_TYPE=Debug|Release] [BUILD_TESTING=|ON|OFF] [USE_PROJ=|ON|OFF] [USE_FFMPEG=|ON|OFF] [FPSDK_VERSION_STRING=x.x.x-gggggggg] [VERBOSE=1]" + @echo " make [INSTALL_PREFIX=...] [BUILD_TYPE=Debug|Release] [BUILD_TESTING=|ON|OFF] [USE_PROJ=|ON|OFF]" + @echo " [USE_FFMPEG=|ON|OFF] [USE_BZ2=|ON|OFF] [FPSDK_VERSION_STRING=x.x.x-gggggggg] [VERBOSE=1]" @echo @echo "Where possible s are:" @echo @@ -138,6 +129,9 @@ endif ifneq ($(USE_FFMPEG),) CMAKE_ARGS += -DFPSDK_USE_FFMPEG=$(USE_FFMPEG) endif +ifneq ($(USE_BZ2),) + CMAKE_ARGS += -DFPSDK_USE_BZ2=$(USE_BZ2) +endif ifneq ($(FPSDK_VERSION_STRING),) CMAKE_ARGS += -DVERSION_STRING=$(FPSDK_VERSION_STRING) endif @@ -204,8 +198,6 @@ $(BUILD_DIR)/.make-cmake: $(deps_cmake) $(BUILD_DIR)/.make-configuid-$(configuid deps_build = $(sort $(wildcard fpsdk.sh fpsdk_doc/* \ fpsdk_common/* fpsdk_common/*/* fpsdk_common/*/*/* fpsdk_common/*/*/*/* \ - fpsdk_ros1/* fpsdk_ros1/*/* fpsdk_ros1/*/*/* fpsdk_ros1/*/*/*/* \ - fpsdk_ros2/* fpsdk_ros2/*/* fpsdk_ros2/*/*/* fpsdk_ros2/*/*/*/* \ fpsdk_apps/* fpsdk_apps/*/* fpsdk_apps/*/*/* fpsdk_apps/*/*/*/*)) .PHONY: build @@ -232,12 +224,6 @@ $(BUILD_DIR)/.make-install: $(BUILD_DIR)/.make-build test: $(BUILD_DIR)/.make-build @echo "$(HLW)***** Test ($(BUILD_TYPE)) *****$(HLO)" $(V)(cd $(BUILD_DIR)/fpsdk_common && ctest $(CTEST_ARGS)) -ifneq ($(FPSDK_USE_ROS1),) - $(V)(cd $(BUILD_DIR)/fpsdk_ros1 && ctest $(CTEST_ARGS)) -endif -ifneq ($(FPSDK_USE_ROS2),) - $(V)(cd $(BUILD_DIR)/fpsdk_ros2 && ctest $(CTEST_ARGS)) -endif # ---------------------------------------------------------------------------------------------------------------------- diff --git a/README.md b/README.md index bd093745..ebaff838 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,5 @@ The documentation (dependencies, build instructions, API, etc.) is in Doxygen. S [fpsdk_doc/fpsdk_doc.hpp](fpsdk_doc/fpsdk_doc.hpp) or find a generated HTML version of the main branch here: https://fixposition.github.io/fixposition-sdk/fixposition-sdk-docs -License: see the LICENSE files: [LICENSE](LICENSE), [fpsdk_common/LICENSE](fpsdk_common/LICENSE), -[fpsdk_ros1/LICENSE](fpsdk_ros1/LICENSE), [fpsdk_ros2/LICENSE](fpsdk_ros2/LICENSE), +License: see the LICENSE files: [LICENSE](LICENSE), [fpsdk_common/LICENSE](fpsdk_common/LICENSE) and [fpsdk_apps/LICENSE](fpsdk_apps/LICENSE). diff --git a/docker/ci.sh b/docker/ci.sh index c65df6ee..84240677 100755 --- a/docker/ci.sh +++ b/docker/ci.sh @@ -239,7 +239,7 @@ function build_toplevel_release_noros_mindeps cmake -B build/${buildname} -S . \ -DCMAKE_INSTALL_PREFIX=install/${buildname} \ -DCMAKE_BUILD_TYPE=Release \ - -DFPSDK_USE_PROJ=OFF -DFPSDK_USE_FFMPEG=OFF || return 1 + -DFPSDK_USE_BZ2=OFF -DFPSDK_USE_PROJ=OFF -DFPSDK_USE_FFMPEG=OFF || return 1 cmake --build build/${buildname} ${CMAKE_BUILD_ARGS} || return 1 cmake --install build/${buildname} || return 1 @@ -259,7 +259,7 @@ function build_projs_release_noros_mindeps cmake -B build/${buildname}/fpsdk_common -S fpsdk_common \ -DCMAKE_INSTALL_PREFIX=install/${buildname} \ -DCMAKE_BUILD_TYPE=Release \ - -DFPSDK_USE_PROJ=OFF -DFPSDK_USE_FFMPEG=OFF || return 1 + -DFPSDK_USE_BZ2=OFF -DFPSDK_USE_PROJ=OFF -DFPSDK_USE_FFMPEG=OFF || return 1 cmake --build build/${buildname}/fpsdk_common ${CMAKE_BUILD_ARGS} || return 1 cmake --install build/${buildname}/fpsdk_common || return 1 @@ -278,7 +278,6 @@ TITLES["build_examples"]="Build examples" function build_examples { local ok=0 - # Note: the fpsdk_ros[12]_demo examples are built elsewhere cd ${FPSDK_SRC_DIR}/examples make build -k @@ -357,12 +356,6 @@ function build_projs_release_ros1 cmake --build build/${buildname}/fpsdk_common ${CMAKE_BUILD_ARGS} || return 1 cmake --install build/${buildname}/fpsdk_common || return 1 - cmake -B build/${buildname}/fpsdk_ros1 -S fpsdk_ros1 \ - -DCMAKE_INSTALL_PREFIX=install/${buildname} \ - -DCMAKE_BUILD_TYPE=Release || return 1 - cmake --build build/${buildname}/fpsdk_ros1 ${CMAKE_BUILD_ARGS} || return 1 - cmake --install build/${buildname}/fpsdk_ros1 || return 1 - cmake -B build/${buildname}/fpsdk_apps -S fpsdk_apps \ -DCMAKE_INSTALL_PREFIX=install/${buildname} \ -DCMAKE_BUILD_TYPE=Release || return 1 @@ -384,9 +377,7 @@ function build_catkin_release mkdir -p build/${buildname}/src cd build/${buildname}/src ln -s ../../../fpsdk_common . - ln -s ../../../fpsdk_ros1 . ln -s ../../../fpsdk_apps . - ln -s ../../../examples/ros1_fpsdk_demo . cd .. catkin init || return 1 catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release || return 1 @@ -482,12 +473,6 @@ function build_projs_release_ros2 cmake --build build/${buildname}/fpsdk_common ${CMAKE_BUILD_ARGS} || return 1 cmake --install build/${buildname}/fpsdk_common || return 1 - cmake -B build/${buildname}/fpsdk_ros2 -S fpsdk_ros2 \ - -DCMAKE_INSTALL_PREFIX=install/${buildname} \ - -DCMAKE_BUILD_TYPE=Release || return 1 - cmake --build build/${buildname}/fpsdk_ros2 ${CMAKE_BUILD_ARGS} || return 1 - cmake --install build/${buildname}/fpsdk_ros2 || return 1 - cmake -B build/${buildname}/fpsdk_apps -S fpsdk_apps \ -DCMAKE_INSTALL_PREFIX=install/${buildname} \ -DCMAKE_BUILD_TYPE=Release || return 1 @@ -509,9 +494,7 @@ function build_colcon_release mkdir -p build/${buildname}/src cd build/${buildname}/src ln -s ../../../fpsdk_common . - ln -s ../../../fpsdk_ros2 . ln -s ../../../fpsdk_apps . - ln -s ../../../examples/ros2_fpsdk_demo . cd .. colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release || return 1 set +u diff --git a/docker/scripts/install_apt_base.sh b/docker/scripts/install_apt_base.sh index df54b961..6e3979f0 100755 --- a/docker/scripts/install_apt_base.sh +++ b/docker/scripts/install_apt_base.sh @@ -34,6 +34,7 @@ packages=$(awk -v filt=${FPSDK_IMAGE%-*} '$1 ~ filt { print $2 }' < - ${catkin_INCLUDE_DIRS} -) - - -# EXECUTABLES ========================================================================================================== - -add_executable(${PROJECT_NAME}_node - src/node_main.cpp -) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_lib ${catkin_LIBRARIES} fpsdk_common fpsdk_ros1) - - -# INSTALL ============================================================================================================== - -install(TARGETS ${PROJECT_NAME}_lib ${PROJECT_NAME}_node - EXPORT ${PROJECT_NAME}_Targets - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY - "launch" - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - - -# TESTS ================================================================================================================ - -if(CATKIN_ENABLE_TESTING) - -# ... - -endif() - -# ====================================================================================================================== diff --git a/examples/ros1_fpsdk_demo/LICENSE b/examples/ros1_fpsdk_demo/LICENSE deleted file mode 100644 index 52b9c765..00000000 --- a/examples/ros1_fpsdk_demo/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) Fixposition AG (www.fixposition.com) and 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. diff --git a/examples/ros1_fpsdk_demo/README.md b/examples/ros1_fpsdk_demo/README.md deleted file mode 100644 index 34f3dbf4..00000000 --- a/examples/ros1_fpsdk_demo/README.md +++ /dev/null @@ -1,40 +0,0 @@ -# Fixposition SDK: ROS1 Demo - -An example ROS1 package with a node demonstrating the use of the Fixpositon SDK. - ---- -## Dependencies - -- [Fixposition SDK dependencies](../../README.md) -- [fpsdk_common](../../fpsdk_common/README.md) -- [fpsdk_ros1](../../fpsdk_ros1/README.md) - - ---- -## Build - -Setup a new ROS1 workspace and build the demo and its dependencies: - -```sh -mkdir -p ws/src -cd ws/src -ln -s ../../fpsdk_common . -ln -s ../../fpsdk_ros1 . -ln -s ../../examples/ros1_fpsdk_demo . -cd .. -catkin init -catkin config --cmake-args -DCMAKE_BUILD_TYPE=Debug -catkin build -``` - -To run the node: - -```sh -source devel/setup.bash -roslaunch ros1_fpsdk_demo node.launch -``` - ---- -## License - -See the [LICENSE](LICENSE) file and [../fpsdk_doc/README.md#license](../fpsdk_doc/README.md#license). diff --git a/examples/ros1_fpsdk_demo/include/ros1_fpsdk_demo/node.hpp b/examples/ros1_fpsdk_demo/include/ros1_fpsdk_demo/node.hpp deleted file mode 100644 index 5bdbd9c6..00000000 --- a/examples/ros1_fpsdk_demo/include/ros1_fpsdk_demo/node.hpp +++ /dev/null @@ -1,69 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 demo node - */ -#ifndef __ROS1_FPSDK_DEMO_NODE_HPP__ -#define __ROS1_FPSDK_DEMO_NODE_HPP__ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include - -/* Fixposition SDK */ -#include - -/* PACKAGE */ - -namespace Ros1FpsdkDemo { -/* ****************************************************************************************************************** */ - -class DemoParams -{ - public: - DemoParams(); - bool LoadFromRos(const std::string& ns); - - double worker1_interval_; - double worker2_interval_; - double timer1_interval_; - double timer2_interval_; - - static constexpr double INTERVAL_MIN = 0.5; - static constexpr double INTERVAL_MAX = 10.0; -}; - -class DemoNode -{ - public: - DemoNode(const DemoParams& params, ros::NodeHandle& nh); - ~DemoNode(); - bool Start(); - void Stop(); - - private: - const DemoParams params_; - ros::NodeHandle nh_; - fpsdk::common::thread::Thread worker1_; - fpsdk::common::thread::Thread worker2_; - ros::Timer timer1_; - ros::Timer timer2_; - ros::Publisher publisher_; - - bool Worker1(); - bool Worker2(); - void Timer1(const ros::TimerEvent& event); - void Timer2(const ros::TimerEvent& event); -}; - -/* ****************************************************************************************************************** */ -} // namespace Ros1FpsdkDemo -#endif // __ROS1_FPSDK_DEMO_NODE_HPP__ diff --git a/examples/ros1_fpsdk_demo/launch/config.yaml b/examples/ros1_fpsdk_demo/launch/config.yaml deleted file mode 100644 index c6b24d59..00000000 --- a/examples/ros1_fpsdk_demo/launch/config.yaml +++ /dev/null @@ -1,4 +0,0 @@ -worker1_interval: 1.5 -worker2_interval: 3.0 -timer1_interval: 2.0 -timer2_interval: 4.0 diff --git a/examples/ros1_fpsdk_demo/launch/console.conf b/examples/ros1_fpsdk_demo/launch/console.conf deleted file mode 100644 index f8251b1b..00000000 --- a/examples/ros1_fpsdk_demo/launch/console.conf +++ /dev/null @@ -1,2 +0,0 @@ -log4j.logger.ros=INFO -log4j.logger.ros.ros1_fpsdk_demo=DEBUG diff --git a/examples/ros1_fpsdk_demo/launch/node.launch b/examples/ros1_fpsdk_demo/launch/node.launch deleted file mode 100644 index d0157aae..00000000 --- a/examples/ros1_fpsdk_demo/launch/node.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/examples/ros1_fpsdk_demo/package.xml b/examples/ros1_fpsdk_demo/package.xml deleted file mode 100644 index 38b9e773..00000000 --- a/examples/ros1_fpsdk_demo/package.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - ros1_fpsdk_demo - 1.0.0 - Fixposition SDK: ROS1 Demo - Fixposition Support - MIT - catkin - fpsdk_common - fpsdk_ros1 - roscpp - std_msgs - diff --git a/examples/ros1_fpsdk_demo/src/node.cpp b/examples/ros1_fpsdk_demo/src/node.cpp deleted file mode 100644 index 5f868ed5..00000000 --- a/examples/ros1_fpsdk_demo/src/node.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 demo node - */ - -/* LIBC/STL */ -#include -#include - -/* EXTERNAL */ -#include -#include -#include - -/* Fixposition SDK */ -#include -#include -#include -#include - -/* PACKAGE */ -#include "ros1_fpsdk_demo/node.hpp" - -namespace Ros1FpsdkDemo { -/* ****************************************************************************************************************** */ - -DemoParams::DemoParams() /* clang-format off */ : - worker1_interval_ { 1.0 }, - worker2_interval_ { 1.0 }, - timer1_interval_ { 1.0 }, - timer2_interval_ { 1.0 } // clang-format on -{ -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoParams::LoadFromRos(const std::string& ns) -{ - bool ok = true; - - if (!fpsdk::ros1::utils::LoadRosParam(ns + "/worker1_interval", worker1_interval_)) { - ROS_WARN("DemoParams: worker1_interval param missing"); - ok = false; - } - if (!fpsdk::ros1::utils::LoadRosParam(ns + "/worker2_interval", worker2_interval_)) { - ROS_WARN("DemoParams: worker2_interval param missing"); - ok = false; - } - if (!fpsdk::ros1::utils::LoadRosParam(ns + "/timer1_interval", timer1_interval_)) { - ROS_WARN("DemoParams: timer1_interval param missing"); - ok = false; - } - if (!fpsdk::ros1::utils::LoadRosParam(ns + "/timer2_interval", timer2_interval_)) { - ROS_WARN("DemoParams: timer2_interval param missing"); - ok = false; - } - - if ((worker1_interval_ < INTERVAL_MIN) || (worker1_interval_ > INTERVAL_MAX)) { - ROS_WARN("DemoParams: Bad value for worker1_interval_ param"); - ok = false; - } - if ((worker2_interval_ < INTERVAL_MIN) || (worker2_interval_ > INTERVAL_MAX)) { - ROS_WARN("DemoParams: Bad value for worker2_interval param"); - ok = false; - } - if ((timer1_interval_ < INTERVAL_MIN) || (timer1_interval_ > INTERVAL_MAX)) { - ROS_WARN("DemoParams: Bad value for timer1_interval param"); - ok = false; - } - if ((timer2_interval_ < INTERVAL_MIN) || (timer2_interval_ > INTERVAL_MAX)) { - ROS_WARN("DemoParams: Bad value for timer2_interval param"); - ok = false; - } - - ROS_INFO("DemoParams: worker1_interval=%.1f worker2_interval=%.1f timer1_interval=%.1f timer2_interval=%.1f", - worker1_interval_, worker2_interval_, timer1_interval_, timer2_interval_); - - return ok; -} - -/* ****************************************************************************************************************** */ - -DemoNode::DemoNode(const DemoParams& params, ros::NodeHandle& nh) /* clang-format off */ : - params_ { params }, - nh_ { nh }, - worker1_ { "worker1", std::bind(&DemoNode::Worker1, this) }, - worker2_ { "worker2", std::bind(&DemoNode::Worker2, this) } // clang-format on -{ - ROS_DEBUG("DemoNode()"); -} - -// --------------------------------------------------------------------------------------------------------------------- - -DemoNode::~DemoNode() -{ - ROS_DEBUG("~DemoNode()"); - Stop(); -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoNode::Start() -{ - ROS_DEBUG("DemoNode::Start()"); - timer1_ = nh_.createTimer(ros::Duration(params_.timer1_interval_), &DemoNode::Timer1, this); - timer2_ = nh_.createTimer(ros::Duration(params_.timer2_interval_), &DemoNode::Timer2, this); - publisher_ = nh_.advertise("string", 5); - return worker1_.Start() && worker2_.Start(); -} - -// --------------------------------------------------------------------------------------------------------------------- - -void DemoNode::Stop() -{ - ROS_DEBUG("DemoNode::Stop()"); - timer1_.stop(); - timer2_.stop(); - if (worker1_.GetStatus() == worker1_.Status::RUNNING) { - worker1_.Stop(); - } - if (worker2_.GetStatus() == worker2_.Status::RUNNING) { - worker2_.Stop(); - } - publisher_.shutdown(); -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoNode::Worker1() -{ - ROS_DEBUG("DemoNode::Worker1() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - while (!worker1_.ShouldAbort()) { - ROS_DEBUG("DemoNode::Worker1() ..."); - std_msgs::String msg; - msg.data = "worker1..."; - publisher_.publish(msg); - ros::Duration(params_.worker1_interval_).sleep(); - } - ROS_DEBUG("DemoNode::Worker1() done"); - return true; -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoNode::Worker2() -{ - ROS_DEBUG("DemoNode::Worker2() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - while (!worker2_.ShouldAbort()) { - ROS_DEBUG("DemoNode::Worker2() ..."); - std_msgs::String msg; - msg.data = "worker2..."; - publisher_.publish(msg); - ros::Duration(params_.worker2_interval_).sleep(); - } - ROS_DEBUG("DemoNode::Worker() done"); - return true; -} - -// --------------------------------------------------------------------------------------------------------------------- - -void DemoNode::Timer1(const ros::TimerEvent& /*event*/) -{ - ROS_DEBUG("DemoNode::Timer1() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - std_msgs::String msg; - msg.data = "timer1..."; - publisher_.publish(msg); -} - -// --------------------------------------------------------------------------------------------------------------------- - -void DemoNode::Timer2(const ros::TimerEvent& /*event*/) -{ - ROS_DEBUG("DemoNode::Timer2() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - std_msgs::String msg; - msg.data = "timer2..."; - publisher_.publish(msg); -} - -/* ****************************************************************************************************************** */ -} // namespace Ros1FpsdkDemo diff --git a/examples/ros1_fpsdk_demo/src/node_main.cpp b/examples/ros1_fpsdk_demo/src/node_main.cpp deleted file mode 100644 index 56b8c785..00000000 --- a/examples/ros1_fpsdk_demo/src/node_main.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 demo node - */ - -/* LIBC/STL */ -#include -#include - -/* EXTERNAL */ -#include -#include - -/* Fixposition SDK */ -#include -#include -#include -#include - -/* PACKAGE */ -#include "ros1_fpsdk_demo/node.hpp" - -/* ****************************************************************************************************************** */ - -int main(int argc, char** argv) -{ -#ifndef NDEBUG - fpsdk::common::app::StacktraceHelper stacktrace; - WARNING("***** Running debug build *****"); -#endif - ROS_INFO("main() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - - bool ok = true; - - // Initialise - ros::init(argc, argv, "ros1_fpsdk_demo_node"); - ros::NodeHandle nh("~"); - - // Redirect Fixposition SDK logging to ROS console, all should use the "ros.ros1_fpsdk_demo" logger - fpsdk::ros1::utils::RedirectLoggingToRosConsole(); - DEBUG("This is a message from fpsdk_common's logging, redirected to the ROS console"); - ROS_DEBUG("This is a proper ROS console message"); - - // Handle CTRL-C / SIGINT ourselves - fpsdk::common::app::SigIntHelper sigint; - - // Load params - Ros1FpsdkDemo::DemoParams params; - if (!params.LoadFromRos("~")) { - ROS_ERROR("Failed loading params"); - ok = false; - } - - // Start node - if (ok) { - Ros1FpsdkDemo::DemoNode node(params, nh); - if (node.Start()) { - ROS_INFO("main() spinning..."); - -#if 1 - // Do the same as ros::spin(), but also handle CTRL-C / SIGINT nicely - // Callbacks execute in main thread - auto cbq = ros::getGlobalCallbackQueue(); - while (ros::ok() && !sigint.ShouldAbort()) { - cbq->callAvailable(ros::WallDuration(0.25)); - } -#else - // Use multiple spinner threads. Callback execute in one of them. - ros::AsyncSpinner spinner{ 4 }; - spinner.start(); - sigint.WaitAbort(); - spinner.stop(); -#endif - - ROS_INFO("main() stopping"); - - } else { - ROS_ERROR("Failed starting node"); - ok = false; - } - - // Cancel any running ros::Duration::sleep() (e.g. in DemoNode::Worker()) - // @todo Why does this not work? - ros::Time::shutdown(); - - // Stop node - node.Stop(); - } - - // Are we happy? - if (ok) { - ROS_INFO("Done"); - } else { - ROS_FATAL("Ouch!"); - } - - // As a very last thing, shutdown ROS - ros::shutdown(); - - exit(ok ? EXIT_SUCCESS : EXIT_FAILURE); -} - -/* ****************************************************************************************************************** */ diff --git a/examples/ros2_fpsdk_demo/CMakeLists.txt b/examples/ros2_fpsdk_demo/CMakeLists.txt deleted file mode 100644 index 0227b843..00000000 --- a/examples/ros2_fpsdk_demo/CMakeLists.txt +++ /dev/null @@ -1,87 +0,0 @@ -# GENERAL ============================================================================================================== -message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----") - -cmake_minimum_required(VERSION 3.16) - -project(ros2_fpsdk_demo - LANGUAGES CXX C - VERSION 1.0.0 - DESCRIPTION "Fixposition SDK: ROS2 Demo" -) - - -# COMPILER SETUP ======================================================================================================= - -# TODO: The ament stuff overrides some of these...? -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror \ - -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual") -set(CMAKE_CXX_FLAGS_RELEASE "-O3") -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() -if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") - add_compile_definitions(NDEBUG) -endif() - - -# DEPENDENCIES ========================================================================================================= - -find_package(ament_cmake REQUIRED) # https://docs.ros.org/en/humble/How-To-Guides/Ament-CMake-Documentation.html -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(fpsdk_common REQUIRED) -find_package(fpsdk_ros2 REQUIRED) -find_package(rosbag2_cpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - - -# include_directories(include ${rclcpp_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ${fpsdk_common_INCLUDE_DIRS}) - - -# SHARED LIBRARY ======================================================================================================= - -add_library(${PROJECT_NAME}_lib SHARED - src/node.cpp -) - -target_link_libraries(${PROJECT_NAME}_lib - rclcpp::rclcpp fpsdk_common fpsdk_ros2 ${std_msgs_TARGETS}) - - -# EXECUTABLES ========================================================================================================== - -add_executable(${PROJECT_NAME}_node - src/node_main.cpp -) - -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME}_lib rclcpp::rclcpp fpsdk_common fpsdk_ros2 ${rosbag2_cpp_TARGETS}) - -target_include_directories(${PROJECT_NAME}_lib - PUBLIC - $ -) - -install(TARGETS - ${PROJECT_NAME}_lib ${PROJECT_NAME}_node - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME}) - - -install(DIRECTORY - "launch" - DESTINATION share/${PROJECT_NAME}/ -) - -# TESTS ================================================================================================================ - -# ... - -# ====================================================================================================================== -ament_package() diff --git a/examples/ros2_fpsdk_demo/LICENSE b/examples/ros2_fpsdk_demo/LICENSE deleted file mode 100644 index 52b9c765..00000000 --- a/examples/ros2_fpsdk_demo/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) Fixposition AG (www.fixposition.com) and 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. diff --git a/examples/ros2_fpsdk_demo/README.md b/examples/ros2_fpsdk_demo/README.md deleted file mode 100644 index 57f00510..00000000 --- a/examples/ros2_fpsdk_demo/README.md +++ /dev/null @@ -1,38 +0,0 @@ -# Fixposition SDK: ROS2 Demo - -An example ROS2 package with a node demonstrating the use of the Fixpositon SDK. - ---- -## Dependencies - -- [Fixposition SDK dependencies](../../README.md) -- [fpsdk_common](../../fpsdk_common/README.md) -- [fpsdk_ros2](../../fpsdk_ros2/README.md) - - ---- -## Build - -Setup a new ROS2 workspace and build the demo and its dependencies: - -```sh -mkdir -p ws/src -cd ws/src -ln -s ../../fpsdk_common . -ln -s ../../fpsdk_ros2 . -ln -s ../../examples/ros2_fpsdk_demo . -cd .. -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug -``` - -To run the node: - -```sh -source install/setup.bash -ros2 launch ros2_fpsdk_demo node.launch -``` - ---- -## License - -See the [LICENSE](LICENSE) file and [../fpsdk_doc/README.md#license](../fpsdk_doc/README.md#license). diff --git a/examples/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp b/examples/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp deleted file mode 100644 index eb3fd210..00000000 --- a/examples/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp +++ /dev/null @@ -1,72 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 demo node - */ -#ifndef __ROS2_FPSDK_DEMO_NODE_HPP__ -#define __ROS2_FPSDK_DEMO_NODE_HPP__ - -/* LIBC/STL */ -#include - -/* EXTERNAL */ -#include -#include - -/* Fixposition SDK */ -#include - -/* PACKAGE */ - -namespace Ros2FpsdkDemo { -/* ****************************************************************************************************************** */ - -class DemoParams -{ - public: - DemoParams(); - bool LoadFromRos(std::shared_ptr nh, const std::string& ns = ""); - - double worker1_interval_; - double worker2_interval_; - double timer1_interval_; - double timer2_interval_; - - static constexpr double INTERVAL_MIN = 0.5; - static constexpr double INTERVAL_MAX = 10.0; -}; - -class DemoNode -{ - public: - DemoNode(std::shared_ptr nh, const DemoParams& params); - ~DemoNode(); - bool Start(); - void Stop(); - - private: - std::shared_ptr nh_; - DemoParams params_; - rclcpp::Logger logger_; - fpsdk::common::thread::Thread worker1_; - fpsdk::common::thread::Thread worker2_; - rclcpp::TimerBase::SharedPtr timer1_; - rclcpp::TimerBase::SharedPtr timer2_; - rclcpp::Publisher::SharedPtr publisher_; - - bool Worker1(); - bool Worker2(); - void Timer1(); - void Timer2(); -}; - -/* ****************************************************************************************************************** */ -} // namespace Ros2FpsdkDemo -#endif // __ROS2_FPSDK_DEMO_NODE_HPP__ diff --git a/examples/ros2_fpsdk_demo/launch/config.yaml b/examples/ros2_fpsdk_demo/launch/config.yaml deleted file mode 100644 index 47a87f11..00000000 --- a/examples/ros2_fpsdk_demo/launch/config.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - worker1_interval: 1.5 # must be a float! - worker2_interval: 3.0 # must be a float! - timer1_interval: 2.0 # must be a float! - timer2_interval: 4.0 # must be a float! diff --git a/examples/ros2_fpsdk_demo/launch/node.launch b/examples/ros2_fpsdk_demo/launch/node.launch deleted file mode 100644 index f19546c0..00000000 --- a/examples/ros2_fpsdk_demo/launch/node.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/examples/ros2_fpsdk_demo/package.xml b/examples/ros2_fpsdk_demo/package.xml deleted file mode 100644 index 830b7d9b..00000000 --- a/examples/ros2_fpsdk_demo/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ros2_fpsdk_demo - 1.0.0 - Fixposition SDK: ROS2 Demo - Fixposition Support - MIT - ament_cmake - - ament_cmake - - fpsdk_common - fpsdk_ros2 - rclcpp - std_msgs - ament_lint_auto - ament_lint_common - builtin_interfaces - rosidl_default_runtime - diff --git a/examples/ros2_fpsdk_demo/src/node.cpp b/examples/ros2_fpsdk_demo/src/node.cpp deleted file mode 100644 index efe4c5a8..00000000 --- a/examples/ros2_fpsdk_demo/src/node.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 demo node - */ - -/* LIBC/STL */ -#include -#include -#include -#include - -/* EXTERNAL */ - -/* Fixposition SDK */ - -/* PACKAGE */ -#include "ros2_fpsdk_demo/node.hpp" - -namespace Ros2FpsdkDemo { -/* ****************************************************************************************************************** */ - -DemoParams::DemoParams() /* clang-format off */ : - worker1_interval_ { 1.0 }, - worker2_interval_ { 1.0 }, - timer1_interval_ { 1.0 }, - timer2_interval_ { 1.0 } // clang-format on -{ -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoParams::LoadFromRos(std::shared_ptr node, const std::string& ns) -{ - bool ok = true; - - // Declare used params, use as default whatever we the object has currently - const std::string WORKER1_INTERVAL_NAME = ns + ".worker1_interval"; - const std::string WORKER2_INTERVAL_NAME = ns + ".worker2_interval"; - const std::string TIMER1_INTERVAL_NAME = ns + ".timer1_interval"; - const std::string TIMER2_INTERVAL_NAME = ns + ".timer2_interval"; - node->declare_parameter(WORKER1_INTERVAL_NAME, worker1_interval_); - node->declare_parameter(WORKER2_INTERVAL_NAME, worker2_interval_); - node->declare_parameter(TIMER1_INTERVAL_NAME, timer1_interval_); - node->declare_parameter(TIMER2_INTERVAL_NAME, timer2_interval_); - - // Load - if (!node->get_parameter(WORKER1_INTERVAL_NAME, worker1_interval_)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: worker1_interval param missing"); - ok = false; - } - if (!node->get_parameter(WORKER2_INTERVAL_NAME, worker2_interval_)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: worker2_interval param missing"); - ok = false; - } - if (!node->get_parameter(TIMER1_INTERVAL_NAME, timer1_interval_)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: timer1_interval param missing"); - ok = false; - } - if (!node->get_parameter(TIMER2_INTERVAL_NAME, timer2_interval_)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: timer2_interval param missing"); - ok = false; - } - - // Check - if ((worker1_interval_ < INTERVAL_MIN) || (worker1_interval_ > INTERVAL_MAX)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: Bad value for worker1_interval_ param"); - ok = false; - } - if ((worker2_interval_ < INTERVAL_MIN) || (worker2_interval_ > INTERVAL_MAX)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: Bad value for worker2_interval param"); - ok = false; - } - if ((timer1_interval_ < INTERVAL_MIN) || (timer1_interval_ > INTERVAL_MAX)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: Bad value for timer1_interval param"); - ok = false; - } - if ((timer2_interval_ < INTERVAL_MIN) || (timer2_interval_ > INTERVAL_MAX)) { - RCLCPP_WARN(node->get_logger(), "DemoParams: Bad value for timer2_interval param"); - ok = false; - } - - RCLCPP_INFO(node->get_logger(), - "DemoParams: worker1_interval=%.1f worker2_interval=%.1f timer1_interval=%.1f timer2_interval=%.1f", - worker1_interval_, worker2_interval_, timer1_interval_, timer2_interval_); - - return ok; -} - -/* ****************************************************************************************************************** */ - -DemoNode::DemoNode(std::shared_ptr nh, const DemoParams& params) /* clang-format off */ : - nh_ { nh }, - params_ { params }, - logger_ { nh_->get_logger() }, - worker1_ { "worker1", std::bind(&DemoNode::Worker1, this) }, - worker2_ { "worker2", std::bind(&DemoNode::Worker2, this) } // clang-format on -{ - RCLCPP_DEBUG(logger_, "i am debug"); - RCLCPP_INFO(logger_, "i am info"); - RCLCPP_WARN(logger_, "i am warn"); - RCLCPP_ERROR(logger_, "i am error"); - RCLCPP_FATAL(logger_, "i am fatal"); -} - -// --------------------------------------------------------------------------------------------------------------------- - -DemoNode::~DemoNode() -{ - Stop(); -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoNode::Start() -{ - RCLCPP_DEBUG(logger_, "DemoNode::Start() namespace=%s name=%s", nh_->get_namespace(), nh_->get_name()); - timer1_ = nh_->create_wall_timer( - rclcpp::Duration::from_seconds(params_.timer1_interval_).to_chrono(), - std::bind(&DemoNode::Timer1, this)); - timer2_ = nh_->create_wall_timer( - rclcpp::Duration::from_seconds(params_.timer2_interval_).to_chrono(), - std::bind(&DemoNode::Timer2, this)); - - const std::string topic_name = std::string(nh_->get_namespace()) + std::string(nh_->get_name()) + "/string"; - RCLCPP_DEBUG(logger_, "topic_name: %s", topic_name.c_str()); - publisher_ = nh_->create_publisher(topic_name, 5); - - return worker1_.Start() && worker2_.Start(); -} - -// --------------------------------------------------------------------------------------------------------------------- - -void DemoNode::Stop() -{ - RCLCPP_DEBUG(logger_, "DemoNode::Stop()"); - // timer1_.stop(); - // timer2_.stop(); - if (worker1_.GetStatus() == worker1_.Status::RUNNING) { - worker1_.Stop(); - } - if (worker2_.GetStatus() == worker2_.Status::RUNNING) { - worker2_.Stop(); - } - // publisher_.shutdown(); -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoNode::Worker1() -{ - RCLCPP_DEBUG(logger_, "DemoNode::Worker1() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - while (!worker1_.ShouldAbort()) { - RCLCPP_DEBUG(logger_, "DemoNode::Worker1() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - auto msg = std_msgs::msg::String(); - msg.data = "worker1..."; - publisher_->publish(msg); - rclcpp::sleep_for( - rclcpp::Duration::from_seconds(params_.worker1_interval_).to_chrono()); - } - RCLCPP_DEBUG(logger_, "DemoNode::Worker1() done"); - return true; -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool DemoNode::Worker2() -{ - RCLCPP_DEBUG(logger_, "DemoNode::Worker2() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - while (!worker2_.ShouldAbort()) { - RCLCPP_DEBUG(logger_, "DemoNode::Worker2() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - auto msg = std_msgs::msg::String(); - msg.data = "worker2..."; - publisher_->publish(msg); - rclcpp::sleep_for( - rclcpp::Duration::from_seconds(params_.worker2_interval_).to_chrono()); - } - RCLCPP_DEBUG(logger_, "DemoNode::Worker() done"); - return true; -} - -// --------------------------------------------------------------------------------------------------------------------- - -void DemoNode::Timer1() -{ - RCLCPP_DEBUG(logger_, "DemoNode::Timer1() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - auto msg = std_msgs::msg::String(); - msg.data = "timer1..."; - publisher_->publish(msg); -} - -// --------------------------------------------------------------------------------------------------------------------- - -void DemoNode::Timer2() -{ - RCLCPP_DEBUG(logger_, "DemoNode::Timer2() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - auto msg = std_msgs::msg::String(); - msg.data = "timer2..."; - publisher_->publish(msg); -} - -/* ****************************************************************************************************************** */ -} // namespace Ros2FpsdkDemo diff --git a/examples/ros2_fpsdk_demo/src/node_main.cpp b/examples/ros2_fpsdk_demo/src/node_main.cpp deleted file mode 100644 index 66186778..00000000 --- a/examples/ros2_fpsdk_demo/src/node_main.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 demo node - */ - -/* LIBC/STL */ -#include -#include -#include - -/* EXTERNAL */ -#include - -/* Fixposition SDK */ -#include -#include -#include -#include - -/* PACKAGE */ -#include "ros2_fpsdk_demo/node.hpp" - -/* ****************************************************************************************************************** */ - -using namespace Ros2FpsdkDemo; - -int main(int argc, char* argv[]) -{ -#ifndef NDEBUG - fpsdk::common::app::StacktraceHelper stacktrace; - WARNING("***** Running debug build *****"); -#endif - INFO("main() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); - - bool ok = true; - - // Initialise ROS, create node handle - rclcpp::init(argc, argv); - auto nh = std::make_shared("ros2_fpsdk_demo"); - auto logger = nh->get_logger(); - - // Redirect Fixposition SDK logging to ROS console, all should use the "ros2_fpsdk_demo" logger - fpsdk::ros2::utils::RedirectLoggingToRosConsole(logger.get_name()); - DEBUG("This is a message from fpsdk_common's logging, redirected to the ROS console"); - RCLCPP_DEBUG(logger, "This is a proper ROS console message"); - - // Load parameters - RCLCPP_INFO(logger, "Loading parameters..."); - DemoParams params; - if (!params.LoadFromRos(nh)) { - RCLCPP_ERROR(logger, "Failed loading parameters"); - ok = false; - } - - // Handle CTRL-C / SIGINT ourselves - fpsdk::common::app::SigIntHelper sigint; - - // Start node - std::unique_ptr node; - if (ok) { - try { - node = std::make_unique(nh, params); - } catch (const std::exception& ex) { - RCLCPP_ERROR(logger, "Failed creating node: %s", ex.what()); - ok = false; - } - } - if (ok) { - RCLCPP_INFO(logger, "Starting node..."); - if (node->Start()) { - RCLCPP_INFO(logger, "main() spinning..."); -#if 1 - // Do the same as rclpp::spin(nh), but also handle CTRL-C / SIGINT nicely - // Callbacks execute in main thread - while (rclcpp::ok() && !sigint.ShouldAbort()) { - rclcpp::spin_until_future_complete( - nh, std::promise().get_future(), std::chrono::milliseconds(345)); - } -#else - - // Use multiple spinner threads. Callback execute in one of them. - // TODO: this (executing in those threads) doesn't seem to work - rclcpp::executors::MultiThreadedExecutor executor{ rclcpp::ExecutorOptions(), 4 }; - executor.add_node(nh); - while (rclcpp::ok() && !sigint.ShouldAbort()) { - executor.spin_once(std::chrono::milliseconds(345)); - } -#endif - RCLCPP_INFO(logger, "main() stopping..."); - } else { - RCLCPP_ERROR(logger, "Failed starting node"); - ok = false; - } - node->Stop(); - node.reset(); - nh.reset(); - } - - // Are we happy? - if (ok) { - RCLCPP_INFO(logger, "Done"); - } else { - RCLCPP_ERROR(logger, "Ouch!"); - } - - // As a very last thing, shutdown ROS - rclcpp::shutdown(); - - exit(ok ? EXIT_SUCCESS : EXIT_FAILURE); -} - -/* ****************************************************************************************************************** */ diff --git a/fpsdk.code-workspace b/fpsdk.code-workspace index 2050a356..a34aa09d 100644 --- a/fpsdk.code-workspace +++ b/fpsdk.code-workspace @@ -58,9 +58,9 @@ // https://code.visualstudio.com/docs/cpp/customize-default-settings-cpp, // https://code.visualstudio.com/docs/cpp/c-cpp-properties-schema-reference) "C_Cpp.default.includePath": [ - "${workspaceFolder}/fpsdk_common/**", "${workspaceFolder}/fpsdk_ros1/**", "${workspaceFolder}/fpsdk_ros2/**", "${workspaceFolder}/fpsdk_apps/**", + "${workspaceFolder}/fpsdk_common/**", "${workspaceFolder}/fpsdk_apps/**", "/opt/ros/noetic/include", "/opt/ros/humble/include", "/opt/ros/jazzy/include", "/opt/ros/lyrical/include" ], - "C_Cpp.default.defines": [ "FPSDK_USE_ROS1" ], + "C_Cpp.default.defines": [ ], //"C_Cpp.default.compileCommands": "", //"C_Cpp.default.forcedIncludes": [ ], "C_Cpp.default.intelliSenseMode": "gcc-x64", diff --git a/fpsdk_apps/CMakeLists.txt b/fpsdk_apps/CMakeLists.txt index 5b5a02f8..35fdeab4 100644 --- a/fpsdk_apps/CMakeLists.txt +++ b/fpsdk_apps/CMakeLists.txt @@ -11,6 +11,9 @@ project(fpsdk_apps DESCRIPTION "Fixposition SDK: Apps" ) +set(_unused "${FPSDK_BUILD_TESTING}") # suppress warning in some setups +set(_unused "${CMAKE_C_COMPILER}") # suppress warning in some setups + # COMPILER SETUP ======================================================================================================= @@ -37,23 +40,21 @@ if(NOT TARGET fpsdk_common) endif() if(FPSDK_USE_ROS1) - if(NOT TARGET fpsdk_ros1) - find_package(fpsdk_ros1 REQUIRED) - endif() - fpsdk_cmake_find_ros1_package(rosbag) - add_compile_definitions(FPSDK_USE_ROS1) + fpsdk_cmake_find_ros1_package(roscpp) + fpsdk_cmake_find_ros1_package(rosconsole) + fpsdk_cmake_find_ros1_package(std_msgs) + fpsdk_cmake_find_ros1_package(sensor_msgs) + fpsdk_cmake_find_ros1_package(geometry_msgs) + fpsdk_cmake_find_ros1_package(tf2_msgs) + fpsdk_cmake_find_ros1_package(nav_msgs) elseif(FPSDK_USE_ROS2) - add_compile_definitions(FPSDK_USE_ROS2) - # Why necessary, what are we doing wrong? See fpsdk_ros2/CMakeLists.txt + find_package(rclcpp REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) find_package(nav_msgs REQUIRED) - if(NOT TARGET fpsdk_ros2) - find_package(fpsdk_ros2 REQUIRED) - endif() else() message(STATUS "fpsdk_apps: No ROS available") endif() @@ -67,12 +68,6 @@ find_package(nlohmann_json REQUIRED) # https://gitlab.kitware.com/cmake/community/-/wikis/doc/cmake/RPATH-handling#recommendations list(APPEND CMAKE_INSTALL_RPATH $ORIGIN) list(APPEND CMAKE_INSTALL_RPATH $ORIGIN/../lib) -if(FPSDK_USE_ROS1) - #list(APPEND CMAKE_INSTALL_RPATH ${fpsdk_ros1_ROS_LIBRARIES_DIRS}) - # Note that user still has to load the ROS environment (setup.bash) as the RUNPATH (mistakenly called RPATH by - # cmake) a) does not work for indirect loading (app -> libfpsdk_ros1 -> someroslib) and b) ROS does funny things - # run-time (such as loading more libs) -endif() # EXECUTABLES ========================================================================================================== @@ -84,10 +79,20 @@ add_executable(fpltool ${FPLTOOL_CPP_FILES}) target_link_libraries(fpltool PRIVATE fpsdk_common - $<$:fpsdk_ros1> - $<$:fpsdk_ros2> - $ + $ + $ + $ + $ + $ + $ + $ + ${rclcpp_TARGETS} ${rosbag2_cpp_TARGETS} + ${std_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${tf2_msgs_TARGETS} + ${nav_msgs_TARGETS} ) file(GLOB PARSERTOOL_CPP_FILES parsertool/*.cpp) diff --git a/fpsdk_apps/fpltool/fpltool_dump.cpp b/fpsdk_apps/fpltool/fpltool_dump.cpp index 54ac0779..0d4b5802 100644 --- a/fpsdk_apps/fpltool/fpltool_dump.cpp +++ b/fpsdk_apps/fpltool/fpltool_dump.cpp @@ -101,7 +101,7 @@ bool DoDump(const FplToolOptions& opts) // Report progress if (opts.progress_ > 0) { if (reader.GetProgress(progress, rate)) { - INFO("Dumping... %.1f%% (%.0f MiB/s)\r", progress, rate); + INFO("Dumping... %.1f%% (%.1f MiB/s)\r", progress, rate); } } diff --git a/fpsdk_apps/fpltool/fpltool_extract.cpp b/fpsdk_apps/fpltool/fpltool_extract.cpp index b1ab3b56..13f0443a 100644 --- a/fpsdk_apps/fpltool/fpltool_extract.cpp +++ b/fpsdk_apps/fpltool/fpltool_extract.cpp @@ -20,12 +20,6 @@ /* EXTERNAL */ #include -#if defined(FPSDK_USE_ROS1) -# include -#elif defined(FPSDK_USE_ROS2) -# include -# include -#endif /* Fixposition SDK */ #include @@ -37,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -64,11 +59,8 @@ using namespace fpsdk::common::string; using namespace fpsdk::common::time; using namespace fpsdk::common::types; using namespace fpsdk::common::video; -#ifdef FPSDK_USE_ROS1 -using namespace fpsdk::ros1::bagwriter; -#endif -#ifdef FPSDK_USE_ROS2 -using namespace fpsdk::ros2::bagwriter; +#if FPSDK_USE_ROS2 +using namespace fpsdk::common::ros2; #endif // --------------------------------------------------------------------------------------------------------------------- @@ -101,25 +93,15 @@ bool FplToolExtract::Run() do_jsonl_ = opts_.formats_.empty(); do_raw_ = opts_.formats_.empty(); do_file_ = opts_.formats_.empty(); -#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) do_ros_ = opts_.formats_.empty(); -#else - do_ros_ = false; -#endif do_cam_ = opts_.formats_.empty(); for (auto& fmt : opts_.formats_) { // clang-format off if (fmt == opts_.FORMAT_JSONL) { do_jsonl_ = true; } else if (fmt == opts_.FORMAT_RAW) { do_raw_ = true; } else if (fmt == opts_.FORMAT_FILE) { do_file_ = true; } else if (fmt == opts_.FORMAT_CAM) { do_cam_ = true; } - else if (fmt == opts_.FORMAT_ROS) { // clang-format on -#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) - do_ros_ = true; -#else - WARNING("Cannot extract to ROS bag. This fpltool is not built with ROS support."); - return false; -#endif - } else { + else if (fmt == opts_.FORMAT_ROS) { do_ros_ = true; } // clang-format on + else { WARNING("Bad argument '%s' to option -e, --formats", fmt.c_str()); return false; } @@ -132,12 +114,11 @@ bool FplToolExtract::Run() NOTICE("Extracting from %s to %s_...", input_fpl.c_str(), output_prefix_.c_str()); TicToc tt; -#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) -# ifdef FPSDK_USE_ROS1 - const auto output_bag = output_prefix_ + ".bag"; // File -# else +#if FPSDK_USE_ROS2 const auto output_bag = output_prefix_ + "_bag"; // Directory! (even for single-file .mcap) -# endif +#else + const auto output_bag = output_prefix_ + ".bag"; // File +#endif if (PathExists(output_bag)) { if (!opts_.overwrite_) { WARNING("Output bag %s already exists", output_bag.c_str()); @@ -146,12 +127,15 @@ bool FplToolExtract::Run() RemoveAll(output_bag); } } +#if FPSDK_USE_ROS2 + if (do_ros_ && !bag_.Open(output_bag, opts_.mcap_, opts_.compress_)) { +#else if (do_ros_ && !bag_.Open(output_bag, opts_.compress_)) { +#endif return false; } NOTICE("Extracting to %s", output_bag.c_str()); -#endif // Handle SIGINT (C-c) to abort nicely SigIntHelper sig_int; @@ -168,7 +152,7 @@ bool FplToolExtract::Run() // Report progress if (opts_.progress_ > 0) { if (fpl_reader.GetProgress(progress, rate)) { - INFO("Extracting... %.1f%% (%.0f MiB/s)\r", progress, rate); + INFO("Extracting... %.1f%% (%.1f MiB/s)\r", progress, rate); } } @@ -219,7 +203,6 @@ bool FplToolExtract::Run() // Close output files CloseAll(ok); -#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) if (do_ros_) { bag_.Close(); if (ok) { @@ -228,7 +211,6 @@ bool FplToolExtract::Run() WARNING("Incomplete bag %s (%s)", output_bag.c_str(), OutputSizeStr(output_bag).c_str()); } } -#endif const auto dur_wall = tt.Toc().GetSec(); const double dur_log = (double)time_into_log - (double)opts_.skip_; @@ -304,11 +286,9 @@ FplToolExtract::ProcRes FplToolExtract::ProcessRosMsgDef(const FplMessage& fpl_m } } -#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) if (do_ros_) { bag_.AddMsgDef(rosmsgdef); } -#endif return ProcRes::OK; } @@ -367,11 +347,9 @@ FplToolExtract::ProcRes FplToolExtract::ProcessRosMsgBin(const FplMessage& fpl_m } } -#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) if (do_ros_ && !bag_.WriteMessage(rosmsgbin)) { return ProcRes::FATAL; } -#endif return ProcRes::OK; } @@ -415,30 +393,23 @@ FplToolExtract::ProcRes FplToolExtract::ProcessStreamMsg(const FplMessage& fpl_m return ProcRes::FATAL; } -#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) if (do_ros_) { -# if defined(FPSDK_USE_ROS1) - std_msgs::ByteMultiArray rosmsg; - ros::Time stamp; -# else +#if FPSDK_USE_ROS2 std_msgs::msg::ByteMultiArray rosmsg; - rclcpp::Time stamp; -# endif +#else + std_msgs::ByteMultiArray rosmsg; +#endif rosmsg.layout.dim.resize(1); rosmsg.layout.dim[0].label = msg.name_; rosmsg.layout.dim[0].size = msg.data_.size(); rosmsg.layout.dim[0].stride = msg.data_.size(); rosmsg.data = { msg.data_.data(), msg.data_.data() + msg.data_.size() }; -# if defined(FPSDK_USE_ROS1) - stamp = { streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_ }; -# else - stamp = { (int)streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_, RCL_ROS_TIME }; -# endif + + const RosTime stamp(streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_); if (!bag_.WriteMessage(rosmsg, "/" + streammsg.stream_name_ + "/raw", stamp)) { return ProcRes::FATAL; } } -#endif } return ProcRes::OK; @@ -500,7 +471,7 @@ FplToolExtract::ProcRes FplToolExtract::ProcessCamData(const FplMessage& fpl_msg ProcRes res = ProcRes::OK; -#if FPSDK_USE_FFMPEG && (defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2)) +#if FPSDK_USE_FFMPEG VideoCodec codec = VideoCodec::UNSPECIFIED; switch (camdata.fmt_) { // clang-format off case CamDataFmt::H264_NAL: codec = VideoCodec::H264; break; @@ -549,7 +520,7 @@ FplToolExtract::ProcRes FplToolExtract::ProcessCamData(const FplMessage& fpl_msg // --------------------------------------------------------------------------------------------------------------------- -#if FPSDK_USE_FFMPEG && (defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2)) +#if FPSDK_USE_FFMPEG FplToolExtract::ProcRes FplToolExtract::ProcessAsyncDecData(const FplToolExtract::AsyncDecData& decdata) { auto& camdata = decdata.camdata_; @@ -560,13 +531,13 @@ FplToolExtract::ProcRes FplToolExtract::ProcessAsyncDecData(const FplToolExtract auto& img = *decdata.img_; TRACE("CAMDATA decoded %s -> %dx%d %s", camdata.info_.c_str(), img.width_, img.height_, PixelFmtToStr(img.fmt_)); -# if defined(FPSDK_USE_ROS1) +# if FPSDK_USE_ROS2 + sensor_msgs::msg::Image rosmsg; + rosmsg.header.stamp = rclcpp::Time(static_cast(camdata.ts_), RCL_ROS_TIME); +# else sensor_msgs::Image rosmsg; rosmsg.header.stamp.fromNSec(camdata.ts_); rosmsg.header.seq = camdata.seq_; -# else - sensor_msgs::msg::Image rosmsg; - rosmsg.header.stamp = rclcpp::Time(static_cast(camdata.ts_), RCL_ROS_TIME); # endif rosmsg.header.frame_id = CamIdToStr(camdata.cam_id_); rosmsg.width = img.width_; @@ -584,12 +555,7 @@ FplToolExtract::ProcRes FplToolExtract::ProcessAsyncDecData(const FplToolExtract const uint32_t dt01ms = camdata.dt_ / 100000; // [ns] -> [0.1ms] rosmsg.data[0] = std::clamp(dt01ms, 0, 255); } -# if defined(FPSDK_USE_ROS1) - const ros::Time stamp(camdata.rec_time_.sec_, camdata.rec_time_.nsec_); -# else - const rclcpp::Time stamp((int)camdata.rec_time_.sec_, camdata.rec_time_.nsec_, RCL_ROS_TIME); -# endif - + const RosTime stamp(camdata.rec_time_.sec_, camdata.rec_time_.nsec_); if (!bag_.WriteMessage(rosmsg, Sprintf("/%s/image", CamIdToStr(camdata.cam_id_)), stamp)) { return ProcRes::FATAL; } diff --git a/fpsdk_apps/fpltool/fpltool_extract.hpp b/fpsdk_apps/fpltool/fpltool_extract.hpp index 9a39cded..150667e6 100644 --- a/fpsdk_apps/fpltool/fpltool_extract.hpp +++ b/fpsdk_apps/fpltool/fpltool_extract.hpp @@ -22,16 +22,13 @@ /* EXTERNAL */ #include -#if defined(FPSDK_USE_ROS1) -# include -#elif defined(FPSDK_USE_ROS2) -# include -#endif /* Fixposition SDK */ #include #include #include +#include +#include #include /* PACKAGE */ @@ -104,10 +101,10 @@ class FplToolExtract #endif // Output files -#if defined(FPSDK_USE_ROS1) - ros1::bagwriter::BagWriter bag_; -#elif defined(FPSDK_USE_ROS2) - ros2::bagwriter::BagWriter bag_; +#if FPSDK_USE_ROS2 + common::ros2::BagWriter bag_; +#else + common::ros1::BagWriter bag_; #endif std::map> files_; common::path::OutputFile* GetOutputFile(const std::string& name); diff --git a/fpsdk_apps/fpltool/fpltool_meta.cpp b/fpsdk_apps/fpltool/fpltool_meta.cpp index 09ed5ff1..d02946b0 100644 --- a/fpsdk_apps/fpltool/fpltool_meta.cpp +++ b/fpsdk_apps/fpltool/fpltool_meta.cpp @@ -62,7 +62,7 @@ bool DoMeta(const FplToolOptions& opts) // Report progress if (opts.progress_ > 0) { if (reader.GetProgress(progress, rate)) { - INFO("Scanning... %.1f%% (%.0f MiB/s)\r", progress, rate); + INFO("Scanning... %.1f%% (%.1f MiB/s)\r", progress, rate); } } diff --git a/fpsdk_apps/fpltool/fpltool_opts.hpp b/fpsdk_apps/fpltool/fpltool_opts.hpp index 7c8bd545..29dcd19d 100644 --- a/fpsdk_apps/fpltool/fpltool_opts.hpp +++ b/fpsdk_apps/fpltool/fpltool_opts.hpp @@ -48,6 +48,7 @@ class FplToolOptions : public common::app::ProgramOptions { 'p', false, "progress" }, { 'P', false, "no-progress" }, { 'c', false, "compress" }, + { 'm', false, "mcap" }, { 'S', true, "skip" }, { 'D', true, "duration" }, { 'e', true, "formats" }, @@ -78,6 +79,9 @@ class FplToolOptions : public common::app::ProgramOptions int extra_ = 0; //!< Enable extra output, such as hexdumps int progress_ = 0; //!< Do progress reports int compress_ = 0; //!< Compress output +#if FPSDK_USE_ROS2 + bool mcap_ = false; //!< Use mcap instead of sqlite3 for ROS2 bags +#endif uint32_t skip_ = 0; //!< Skip start [sec] uint32_t duration_ = 0; //!< Duration [sec] std::vector formats_; //!< List of output formats for extraction @@ -112,7 +116,8 @@ class FplToolOptions : public common::app::ProgramOptions " -p, --progress -- Show progress (default: automatic)\n" " -P, --no-progress -- Don't show progress (default: automatic)\n" " -f, --force -- Force overwrite output (default: refuse to overwrite existing output files)\n" - " -c, --compress -- Compress output (e.g. ROS bags), -c -c to compress more\n" + " -c, --compress -- Compress output (e.g. ROS bags), -c -c to compress more (in some cases)\n" + " -m, --mcap -- Use mcap instead of sqlite3 for ROS2 bags\n" " -x, --extra -- Add extra output, multiple -x can be given\n" " -o, --output -- Output file prefix (default: derive from name)\n" " -S, --skip -- Skip seconds from start of log (default: 0, i.e. no skip)\n" @@ -165,10 +170,11 @@ class FplToolOptions : public common::app::ProgramOptions " jsonl -- All data in JSONL format (see below)\n" " raw -- Stream messages (I/O messages, raw messages from GNSS receiver, raw camera data, ...)\n" " file -- Recorded files (configuration, ...)\n" - " ros -- ROS data extracted to a ROS bag. This option is only available when compiled with ROS\n" - " (1 or 2) support (see output of 'fpltool -V' to check what your version is). For ROS1\n" - " the standard .bag file format is used. For ROS2 the standard sqlite3 or, with compression,\n" - " mcap format is used.\n" + " ros -- ROS data extracted to a ROS bag. Without ROS2 support (see output of 'fpltool -V' to\n" + " check what your version is) a ROS1 bag (the standard .bag file format) is written. This\n" + " works also if this fpltool is built without any ROS support. With -c these bags are bz2\n" + " compressed (if bz2 support is compiled in, see 'fpltool -V'). With ROS2 support a\n" + " ROS2 bag (the standard sqlite3 or, with -m, mcap format, which can use -c) is written.\n" " cam -- Camera data, such as encoded video frames (for example, from PBx-A1). With 'ros' and if\n" " compiled with FFmpeg support (see 'fpltool -V' to check what your version is) encoded\n" " video is decoded and stored as image to the ROS bag. In this case the mapping of the\n" @@ -240,10 +246,10 @@ class FplToolOptions : public common::app::ProgramOptions "\n" " Create a ROS some.bag file (ROS1) resp. some_bag directory (ROS2) from a .fpl file:\n" "\n" - " fpltool extract -e ros some.fpl\n" + " fpltool extract -e ros,cam some.fpl\n" " fpltool robag some.fpl # shortcut\n" "\n" - " Create a compressed another.bag (ROS1, another_bag/ for ROS2) with 2 minutes of data starting 60 seconds\n" + " Create another.bag (ROS1, another_bag/ for ROS2) with 2 minutes of data starting 60 seconds\n" " into some.fpl:\n" "\n" " fpltool rosbag some.fpl -c -c -o another.bag -S 60 -D 120\n" @@ -252,7 +258,7 @@ class FplToolOptions : public common::app::ProgramOptions "\n" " rosbag info some.bag # ROS 1\n" " ros2 bag info some_bag # ROS 2 (default, see above)\n" - " mcap info some_bag/some.mcap # ROS 2 (with compression, see above)\n" + " mcap info some_bag/some.mcap # ROS 2 (with -m, see above)\n" "\n" " Create a ROS bag, convert encoded video (e.g. from PBx-A1 sensor) to greyscale images of half size:\n" "\n" @@ -322,6 +328,14 @@ class FplToolOptions : public common::app::ProgramOptions case 'c': compress_++; break; + case 'm': +#if FPSDK_USE_ROS2 + mcap_ = true; +#else + WARNING("Cannot use --mpcap, this fpltool is not compiled with ROS2"); + ok = false; +#endif + break; case 'S': if (!common::string::StrToValue(argument, skip_)) { ok = false; @@ -336,19 +350,27 @@ class FplToolOptions : public common::app::ProgramOptions formats_ = common::string::StrSplit(argument, ","); break; } -#if FPSDK_USE_FFMPEG case 's': +#if FPSDK_USE_FFMPEG if (!common::string::StrToValue(argument, scale_) || (scale_ < 0.1) || (scale_ > 1.0)) { ok = false; } +#else + WARNING("Cannot use --scale, this fpltool is not compiled with FFmpeg"); + ok = false; +#endif break; case 't': +#if FPSDK_USE_FFMPEG pixelfmt_ = PixelFmtFromStrOr(argument.c_str(), common::video::PixelFmt::UNSPECIFIED); if (pixelfmt_ == common::video::PixelFmt::UNSPECIFIED) { ok = false; } - break; +#else + WARNING("Cannot use --pixelfmt, this fpltool is not compiled with FFmpeg"); + ok = false; #endif + break; default: ok = false; break; @@ -400,6 +422,9 @@ class FplToolOptions : public common::app::ProgramOptions DEBUG("extra = %d", extra_); DEBUG("progress = %d", progress_); DEBUG("compress = %d", compress_); +#if FPSDK_USE_ROS2 + DEBUG("mcap = %d", mcap_); +#endif DEBUG("skip = %d", skip_); DEBUG("duration = %d", duration_); DEBUG("formats = %s", common::string::StrJoin(formats_, " ").c_str()); diff --git a/fpsdk_apps/fpltool/fpltool_trim.cpp b/fpsdk_apps/fpltool/fpltool_trim.cpp index 25a7c409..c472294a 100644 --- a/fpsdk_apps/fpltool/fpltool_trim.cpp +++ b/fpsdk_apps/fpltool/fpltool_trim.cpp @@ -92,7 +92,7 @@ bool DoTrim(const FplToolOptions& opts) // Report progress if (opts.progress_ > 0) { if (reader.GetProgress(progress, rate)) { - INFO("Processing... %.1f%% (%.0f MiB/s) -- time into log %" PRIu32 ", used %" PRIu64 " msgs\r", + INFO("Processing... %.1f%% (%.1f MiB/s) -- time into log %" PRIu32 ", used %" PRIu64 " msgs\r", progress, rate, time_into_log, n_used); } } diff --git a/fpsdk_apps/package.xml b/fpsdk_apps/package.xml index ae469c25..e6e789c7 100644 --- a/fpsdk_apps/package.xml +++ b/fpsdk_apps/package.xml @@ -9,8 +9,4 @@ cmake fpsdk_common - - fpsdk_ros1 - fpsdk_ros2 diff --git a/fpsdk_common/CMakeLists.txt b/fpsdk_common/CMakeLists.txt index 47998b32..ee5e1a39 100644 --- a/fpsdk_common/CMakeLists.txt +++ b/fpsdk_common/CMakeLists.txt @@ -11,6 +11,7 @@ project(fpsdk_common DESCRIPTION "Fixposition SDK: Common Library" ) +set(_unused "${CMAKE_C_COMPILER}") # suppress warning in some setups # COMPILER SETUP ======================================================================================================= @@ -59,20 +60,30 @@ find_package(nlohmann_json REQUIRED) find_package(Threads REQUIRED) find_package(PkgConfig REQUIRED) pkg_search_module(OpenSSL REQUIRED IMPORTED_TARGET openssl) +fpsdk_find_package_bz2() fpsdk_find_package_proj() fpsdk_find_package_ffmpeg() if(FPSDK_USE_ROS1) + fpsdk_cmake_find_ros1_package(roscpp) + fpsdk_cmake_find_ros1_package(rosconsole) fpsdk_cmake_find_ros1_package(std_msgs) fpsdk_cmake_find_ros1_package(sensor_msgs) fpsdk_cmake_find_ros1_package(geometry_msgs) fpsdk_cmake_find_ros1_package(tf2_msgs) fpsdk_cmake_find_ros1_package(nav_msgs) - add_compile_definitions(FPSDK_USE_ROS1) else() if(FPSDK_USE_ROS2) - add_compile_definitions(FPSDK_USE_ROS2) + find_package(rclcpp REQUIRED) + find_package(rosbag2_cpp REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(tf2_msgs REQUIRED) + find_package(nav_msgs REQUIRED) endif() + + # Minimal ROS1 stuff needed for fpltool fpsdk_cmake_local_ros1_package(std_msgs) fpsdk_cmake_local_ros1_package(sensor_msgs) fpsdk_cmake_local_ros1_package(geometry_msgs) @@ -81,13 +92,39 @@ else() endif() fpsdk_save_versions(FILE ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_versions.txt - PACKAGES CMAKE Boost yaml-cpp ZLIB Eigen3 nlohmann_json OpenSSL PROJ libavcodec libavutil libavfilter libswscale) + PACKAGES CMAKE Boost yaml-cpp ZLIB BZIP2 Eigen3 nlohmann_json OpenSSL PROJ libavcodec libavutil libavfilter libswscale) # SHARED LIBRARY ======================================================================================================= file(GLOB CPP_FILES src/*.cpp src/parser/*.cpp src/stream/*.cpp) + +# Unless building on a real ROS1 system we have to build some minimal implementation. Most things are header-only, +# though. See the fpsdk_cmake_local_ros1_package() and the setup for the rosnoros/ headers below. +if(NOT FPSDK_USE_ROS1) + file(GLOB ROSNOROS_CPP_FILES rosnoros/ros/*.cpp) + list(APPEND CPP_FILES ${ROSNOROS_CPP_FILES}) +endif() + add_library(${PROJECT_NAME} SHARED ${CPP_FILES}) +if(FPSDK_USE_ROS1) + target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_ROS1=1) +else() + target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_ROS1=0) +endif() + +if(FPSDK_USE_ROS2) + target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_ROS2=1) +else() + target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_ROS2=0) +endif() + +if(FPSDK_USE_BZ2) + target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_BZ2=1) +else() + target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_BZ2=0) +endif() + if(FPSDK_USE_PROJ) target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_PROJ=1) else() @@ -120,7 +157,10 @@ target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen yaml-cpp + Threads::Threads PRIVATE + $ + $ $ $ $ @@ -130,13 +170,20 @@ target_link_libraries(${PROJECT_NAME} ${CMAKE_DL_LIBS} ZLIB::ZLIB PkgConfig::OpenSSL - Threads::Threads nlohmann_json::nlohmann_json $<$:PROJ::proj> $<$:PkgConfig::libavcodec> $<$:PkgConfig::libavutil> $<$:PkgConfig::libavfilter> $<$:PkgConfig::libswscale> + $<$:BZip2::BZip2> + ${rclcpp_TARGETS} + ${rosbag2_cpp_TARGETS} + ${std_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${tf2_msgs_TARGETS} + ${nav_msgs_TARGETS} ) set_target_properties(${PROJECT_NAME} @@ -161,6 +208,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ install(DIRECTORY rosnoros DESTINATION ${PROJECT_INCLUDE_DIR} + FILES_MATCHING PATTERN "*.h" ) # Library, tools @@ -249,7 +297,8 @@ add_gtest(TARGET parser_types_test SOURCES test/parser_types_test.cpp LINK add_gtest(TARGET parser_ubx_test SOURCES test/parser_ubx_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET parser_unib_test SOURCES test/parser_unib_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET path_test SOURCES test/path_test.cpp LINK_LIBS ${PROJECT_NAME}) -add_gtest(TARGET ros1_test SOURCES test/ros1_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::sensor_msgs ros1::tf2_msgs) +add_gtest(TARGET ros1_test SOURCES test/ros1_test.cpp LINK_LIBS ${PROJECT_NAME} $ $ $ $ $ $ $ $<$:BZip2::BZip2>) +add_gtest(TARGET ros2_test SOURCES test/ros2_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS} ${rosbag2_cpp_TARGETS} ${std_msgs_TARGETS} ${sensor_msgs_TARGETS} ${geometry_msgs_TARGETS} ${tf2_msgs_TARGETS} ${nav_msgs_TARGETS}) add_gtest(TARGET string_test SOURCES test/string_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET time_test SOURCES test/time_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET thread_test SOURCES test/thread_test.cpp LINK_LIBS ${PROJECT_NAME}) diff --git a/fpsdk_common/LICENSE b/fpsdk_common/LICENSE index 05e4486e..adce71f8 100644 --- a/fpsdk_common/LICENSE +++ b/fpsdk_common/LICENSE @@ -82,7 +82,8 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -------------------------------------------------------------------------------- -Some of fpsdk_common uses FFmpeg libraries (libavcodec, libavutil and libswscale) +Some of fpsdk_common optionally uses FFmpeg libraries (libavcodec, libavutil, +libavfilter, libswscale), built with --disable-gpl and --disable-nonfree, licensed under the GNU Lesser General Public License version 2.1 or later. Source code for these libraries is available at: https://ffmpeg.org/ diff --git a/fpsdk_common/cmake/setup.cmake b/fpsdk_common/cmake/setup.cmake index 628bd9e8..76b10040 100644 --- a/fpsdk_common/cmake/setup.cmake +++ b/fpsdk_common/cmake/setup.cmake @@ -22,8 +22,8 @@ endif() ######################################################################################################################## -# Check if we have ROS, and add to cmake path to that require(rosbag) works. User can either specify it as a cmake -# argument or we can detect it from the environment +# Check if we have ROS, and add to cmake path to that. User can either specify it as a cmake argument or we can detect +# it from the environment # - ROS2 environment loaded if("$ENV{ROS_VERSION}" STREQUAL "2") message(STATUS "fpsdk: Using ROS2") diff --git a/fpsdk_common/cmake/utils.cmake b/fpsdk_common/cmake/utils.cmake index 3bf5b145..5485834b 100644 --- a/fpsdk_common/cmake/utils.cmake +++ b/fpsdk_common/cmake/utils.cmake @@ -22,6 +22,38 @@ macro(fpsdk_cmake_local_ros1_package ROS_PACKAGE) endif() endmacro() +######################################################################################################################## + +# bzip2 is optional, unless explicitly requested with -DFPSDK_USE_BZ2=ON. It is used for compressing ROS1 bags. +macro(fpsdk_find_package_bz2) + # message(STATUS "fpsdk: FPSDK_USE_BZ2=${FPSDK_USE_BZ2}") + + # Explicitly requested by user to use bzip2 + if(FPSDK_USE_BZ2 STREQUAL "ON") + find_package(BZip2 REQUIRED) + + # Explicitly requested by user to not use bzip2 + elseif(FPSDK_USE_BZ2 STREQUAL "OFF") + message(STATUS "fpsdk: Not using bzip2") + + # Automatic, use bzip2 if it is available + else() + find_package(BZip2 QUIET) + if(${BZIP2_FOUND}) + set(FPSDK_USE_BZ2 ON) + else() + message(STATUS "fpsdk: No bzip2 found") + set(FPSDK_USE_BZ2 OFF) + endif() + endif() + + if(FPSDK_USE_BZ2) + message(STATUS "fpsdk: Using bzip2 (${BZIP2_VERSION_STRING}, ${BZIP2_INCLUDE_DIRS})") + endif() + +endmacro() + + ######################################################################################################################## # PROJ is optional, unless explicitly requested with -DFPSDK_USE_PROJ=ON diff --git a/fpsdk_common/doc/doc.hpp b/fpsdk_common/doc/doc.hpp index 980503b0..2c421afa 100644 --- a/fpsdk_common/doc/doc.hpp +++ b/fpsdk_common/doc/doc.hpp @@ -41,6 +41,7 @@ namespace common { - @subpage FPSDK_COMMON_PARSER - @subpage FPSDK_COMMON_PATH - @subpage FPSDK_COMMON_ROS1 + - @subpage FPSDK_COMMON_ROS2 - @subpage FPSDK_COMMON_STRING - @subpage FPSDK_COMMON_THREAD - @subpage FPSDK_COMMON_TIME diff --git a/fpsdk_common/include/fpsdk_common/logging.hpp b/fpsdk_common/include/fpsdk_common/logging.hpp index b4079c78..45db862a 100644 --- a/fpsdk_common/include/fpsdk_common/logging.hpp +++ b/fpsdk_common/include/fpsdk_common/logging.hpp @@ -193,7 +193,7 @@ namespace logging { * The logging levels loosely follow syslog levels (indicated in [] below, see also * https://en.wikipedia.org/wiki/Syslog) * - * Libraries (fpsdk_common, fpsdk_ros1, ...) code shall only use WARNING and DEBUG. + * Libraries (fpsdk_common, ...) code shall only use WARNING and DEBUG. */ enum class LoggingLevel : int { // clang-format off diff --git a/fpsdk_common/include/fpsdk_common/parser.hpp b/fpsdk_common/include/fpsdk_common/parser.hpp index 6d101c05..ee837ca5 100644 --- a/fpsdk_common/include/fpsdk_common/parser.hpp +++ b/fpsdk_common/include/fpsdk_common/parser.hpp @@ -99,7 +99,7 @@ * @section FPSDK_COMMON_PARSER_NAMING Protocol and message naming * * The protocols names are defined in #fpsdk::common::parser::Protocol and can be stringified using - * fpsdk::common::parser::ProtocolStr(). The names must match `/^[A-Z][A-Z0-9_]{2,5}$/. + * fpsdk::common::parser::ProtocolStr(). The names must match `/^[A-Z][A-Z0-9_]{2,5}$/`. * * The message naming scheme consists of words separated by dashes. The first word is always the protocol name. * Depending on the protocol one or two more words are added. All words must match `/^[A-Z][A-Z0-9]{2,9}$/`. diff --git a/fpsdk_common/include/fpsdk_common/ros1.hpp b/fpsdk_common/include/fpsdk_common/ros1.hpp index 6e01c32d..4ae12fe6 100644 --- a/fpsdk_common/include/fpsdk_common/ros1.hpp +++ b/fpsdk_common/include/fpsdk_common/ros1.hpp @@ -14,28 +14,39 @@ * * **API**: fpsdk_common/ros1.hpp and fpsdk::common::ros1 * + * @note Some of this always available, even when built in a non-ROS or ROS2 environment, some of it is only available + * when built in a ROS1 environment, see @ref FPSDK_BUILD_DEPS. */ #ifndef __FPSDK_COMMON_ROS1_HPP__ #define __FPSDK_COMMON_ROS1_HPP__ /* LIBC/STL */ #include +#include #include +#include +#include /* EXTERNAL */ #include -/* ROS */ +/* ROS1 from fpsdk_common/rosnoros or real ROS1 */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #pragma GCC diagnostic ignored "-Wunused-parameter" #pragma GCC diagnostic ignored "-Wshadow" #pragma GCC diagnostic ignored "-Wunused-function" // +#if FPSDK_USE_ROS1 +# include +# include +#endif +// #include #include #include #include +#include #include // #include @@ -43,6 +54,8 @@ #pragma GCC diagnostic pop /* PACKAGE */ +#include "fpl.hpp" +#include "time.hpp" namespace fpsdk { namespace common { @@ -123,6 +136,163 @@ inline void DeserializeMessage(const std::vector& buf, RosMsgT& msg) #endif } +/** + * @brief Convert to ROS time (atomic -> POSIX) + * + * @param[in] time The Time object (atomic) + * + * @returns the ROS time object (POSIX) + */ +ros::Time ConvTime(const time::Time& time); + +/** + * @brief Convert from ROS time (POSIX -> atomic) + * + * @param[in] time The ROS time object (POSIX) + * + * @returns the Time object (atomic) + */ +time::Time ConvTime(const ros::Time& time); + +/** + * @brief ROS1 bag (rosbag) writer + * + * This writes ROS1 bag files (rosbag format version 2.0). Unlike the rosbag library from ROS1 this does not need ROS + * and therefore it works in all builds of the SDK (with ROS1, with ROS2, and without any ROS). Messages can either be + * given as ROS1 message objects (see WriteMessage()) or as already serialised data from a .fpl logfile (see + * AddMsgDef() and WriteMessage()). + * + * Compared to the original rosbag library this implementation has some limitations: + * + * - Only "bz2" chunk compression is available (and only if compiled with BZip2, see @ref FPSDK_BUILD_DEPS) + * - No encryption + * - Only one connection per topic + * - Write only (no reading, no appending to existing bags) + * + * The resulting bags are valid rosbag version 2.0 files and can be read by any ROS1 tool (rosbag info, rosbag play, + * rqt_bag, ...) as well as other tools that understand the format. + */ +class BagWriter +{ + public: + BagWriter(); + ~BagWriter(); + + BagWriter(const BagWriter&) = delete; //!< No copy + BagWriter& operator=(const BagWriter&) = delete; //!< No copy + + /** + * @brief Open bag for writing + * + * @param[in] path Path/filename of the bag file (an existing file is overwritten) + * @param[in] compress Compress bag, 0 = no compression, 1+ = BZ2 (if compiled in) + * + * @returns true if bag was sucessfully opened, false otherwise (bad path, compression requested but bz2 support not + * compiled in, ...) + */ + bool Open(const std::string& path, const int compress = 0); + + /** + * @brief Close bag + * + * @note Failing to close the bag (for example, by not destroying the object) leaves an unusable (unindexed) file + * behind. The index is only written on close. + */ + void Close(); + + /** + * @brief Write a message to the bag + * + * @tparam T ROS message type + * @param[in] msg The message + * @param[in] topic Topic name + * @param[in] time Bag record time + * + * @returns true if message was added, false otherwise (bag not open, write error) + */ + template + bool WriteMessage(const T& msg, const std::string& topic, const time::RosTime& time = {}) + { + const uint32_t size = ros::serialization::serializationLength(msg); + std::vector data(size); + if (size > 0) { + ros::serialization::OStream stream(data.data(), size); + ros::serialization::serialize(stream, msg); + } + return WriteSerialised(topic, time, ros::message_traits::datatype(), ros::message_traits::md5sum(), + ros::message_traits::definition(), data.data(), data.size()); + } + + /** + * @brief Write a message to the bag + * + * @tparam T ROS message type + * @param[in] msg The message + * @param[in] topic Topic name + * @param[in] time Bag record time + * + * @returns true if message was added, false otherwise (bag not open, write error) + */ + template + bool WriteMessage(const T& msg, const std::string& topic, const ros::Time& time) + { + return WriteMessage(msg, topic, time::RosTime(time.sec, time.nsec)); + } + + /** + * @brief Add ROS message definition from .fpl + * + * @note No checks on the provided data are done! + * + * @param[in] rosmsgdef The message definition + */ + void AddMsgDef(const fpl::RosMsgDef& rosmsgdef); + + /** + * @brief Write message from .fpl + * + * @note No checks on the provided data are done! + * + * @param[in] rosmsgbin The recorded message + * + * @returns true if message was added, false otherwise (message definition missing, bag not open, write error) + */ + bool WriteMessage(const fpl::RosMsgBin& rosmsgbin); + + private: +#ifndef _DOXYGEN_ + struct Impl; // The actual bag file writer, see ros1.cpp + std::unique_ptr impl_; + + bool WriteSerialised(const std::string& topic, const time::RosTime& time, const std::string& msg_name, + const std::string& msg_md5, const std::string& msg_def, const uint8_t* data, const std::size_t size); +#endif // _DOXYGEN_ +}; + +#if FPSDK_USE_ROS1 || defined(_DOXYGEN_) +/** + * @brief Redirect fpsdk:common::logging to ROS console + * + * @note This is only available when built in a ROS1 environment. + * + * This configures the fpsdk::common::logging facility to output via the ROS console. This does *not* configure the ROS + * console (logger level, logger name, etc.). + * + * The mapping of fpsdk::common::logging::LoggingLevel to ros::console::levels is as follows: + * + * - TRACE and DEBUG --> DEBUG + * - INFO and NOTICE --> INFO + * - WARNING --> WARN + * - ERROR --> ERROR + * - FATAL --> FATAL + * + * @param[in] logger_name The name of the logger. The default value should give the caller package's + * ROSCONSOLE_DEFAULT_NAME, for example, "ros1_fpsdk_demo". That is, typically this argument + * should be left empty (the default value). + */ +void RedirectLoggingToRosConsole(const char* logger_name = ROSCONSOLE_DEFAULT_NAME /* = caller's package name */); +#endif + /* ****************************************************************************************************************** */ } // namespace ros1 } // namespace common diff --git a/fpsdk_ros2/include/fpsdk_ros2/ros1.hpp b/fpsdk_common/include/fpsdk_common/ros2.hpp similarity index 52% rename from fpsdk_ros2/include/fpsdk_ros2/ros1.hpp rename to fpsdk_common/include/fpsdk_common/ros2.hpp index c86aa1c4..569551b6 100644 --- a/fpsdk_ros2/include/fpsdk_ros2/ros1.hpp +++ b/fpsdk_common/include/fpsdk_common/ros2.hpp @@ -2,41 +2,187 @@ * \verbatim * ___ ___ * \ \ / / - * \ \/ / Copyright (c) Fixposition AG + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors * / /\ \ License: see the LICENSE file * /__/ \__\ * \endverbatim * * @file - * @brief Fixposition SDK: ROS2 types conversion from ROS1 + * @brief Fixposition SDK: ROS2 types and utils * - * @page FPSDK_ROS2_ROS1 ROS2 types conversion from ROS1 + * @page FPSDK_COMMON_ROS2 ROS2 types and utils * - * **API**: fpsdk_ros2/ros1.hpp and fpsdk::ros2::ros1 + * **API**: fpsdk_common/ros2.hpp and fpsdk::common::ros2 * + * This is only available when built in a ROS2 environment. */ -#ifndef __FPSDK_ROS2_ROS1_HPP__ -#define __FPSDK_ROS2_ROS1_HPP__ +#ifndef __FPSDK_COMMON_ROS2_HPP__ +#define __FPSDK_COMMON_ROS2_HPP__ +#if FPSDK_USE_ROS2 || defined(_DOXYGEN_) /* LIBC/STL */ -#include +# include +# include /* EXTERNAL */ -#include -/* Fixposition SDK */ -#include +/* ROS2 */ +# pragma GCC diagnostic push +// #pragma GCC diagnostic ignored "-Wpedantic" +// #pragma GCC diagnostic ignored "-Wunused-parameter" +# pragma GCC diagnostic ignored "-Wshadow" +# include +// +# include +# include +// +# include +# include +# include +# include +# include +# include +# pragma GCC diagnostic pop /* PACKAGE */ +# include "fpsdk_common/ros1.hpp" +# include "fpsdk_common/time.hpp" namespace fpsdk { -namespace ros2 { +namespace common { /** - * @brief ROS2 types conversion from ROS1 + * @brief ROS2 types and utils */ -namespace ros1 { +namespace ros2 { /* ****************************************************************************************************************** */ -#ifdef _DOXYGEN_ + +/** + * @brief Redirect fp:common::logging to ROS console + * + * This configures the fpsdk::common::logging facility to output via the ROS console. This does *not* configure the ROS + * console (logger level, logger name, etc.). + * + * The mapping of fpsdk::common::logging::LoggingLevel to rclcpp levels is as follows: + * + * - TRACE and DEBUG --> DEBUG + * - INFO and NOTICE --> INFO + * - WARNING --> WARN + * - ERROR --> ERROR + * - FATAL --> FATAL + * + * @param[in] logger_name The name of the logger. The recommended value is node->get_logger().get_name() + */ +void RedirectLoggingToRosConsole(const char* logger_name = "fpsdk_common"); + +/** + * @brief Convert to ROS time (atomic -> POSIX) + * + * @param[in] time The Time object (atomic) + * @param[in] clock_type The clock to use (to assume) + * + * @returns the ROS time object (POSIX) + */ +rclcpp::Time ConvTime(const fpsdk::common::time::Time& time, rcl_clock_type_t clock_type = RCL_ROS_TIME); + +/** + * @brief Convert from ROS time (POSIX -> atomic) + * + * @param[in] time The ROS time object (POSIX) + * + * @returns the Time object (atomic) + */ +fpsdk::common::time::Time ConvTime(const rclcpp::Time& time); + +/** + * @brief ROS2 bag writer helper + */ +class BagWriter +{ + public: + BagWriter(); + ~BagWriter(); + + /** + * @brief Open bag for writing + * + * @param[in] path Path of the bag directory + * @param[in] mcap Use mcap instead of sqlite3 format + * @param[in] compress Compress bag more (only with mcap = true), 0 = zstd_small, >=1 = zstd_fast, + * ignored with mcap = false + * + * @returns true if bag was sucessfully opened + */ + bool Open(const std::string& path, const bool mcap = false, const int compress = 0); + + /** + * @brief Close bag + */ + void Close(); + + /** + * @brief Write a message to the bag + * + * @tparam T ROS message type + * @param[in] msg The message + * @param[in] topic Topic name + * @param[in] time Bag record time + * + * @returns true if message was added, false otherwise (message definition missing) + */ + template + bool WriteMessage(const T& msg, const std::string& topic, const rclcpp::Time& time) + { + bool ok = false; + try { + if (bag_) { + bag_->write(msg, topic, time); + ok = true; + } + } catch (const std::exception& ex) { + WARNING("BagWriter: write fail: %s", ex.what()); + } + return ok; + } + + /** + * @brief Write a message to the bag + * + * @tparam T ROS message type + * @param[in] msg The message + * @param[in] topic Topic name + * @param[in] time Bag record time + */ + template + bool WriteMessage(const T& msg, const std::string& topic, const common::time::RosTime& time) + { + return WriteMessage(msg, topic, rclcpp::Time(time.sec_, time.nsec_, RCL_ROS_TIME)); + } + + /** + * @brief Add ROS message definition from .fpl + * + * @note No checks on the provided data are done! + * + * @param[in] rosmsgdef The message definition + */ + void AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef); + + /** + * @brief Write message from .fpl + * + * @note No checks on the provided data are done! + * + * @param[in] rosmsgbin The recorded message + * + * @returns true if message was added, false otherwise (e.g. ROS1->ROS2 conversion not implemented) + */ + bool WriteMessage(const common::fpl::RosMsgBin& rosmsgbin); + + private: + std::unique_ptr bag_; //!< Bag file handle + std::map defs_; //!< Message definitions (connection headers) +}; +# ifdef _DOXYGEN_ // Dummy documentation /** @@ -52,7 +198,7 @@ namespace ros1 { template void Ros1ToRos2(Ros1MsgT& ros1, Ros2MsgT& ros2); -#else +# else inline void Ros1ToRos2(const ros::Time& ros1, builtin_interfaces::msg::Time& ros2) { @@ -196,9 +342,11 @@ inline void Ros1ToRos2(const tf2_msgs::TFMessage& ros1, tf2_msgs::msg::TFMessage } } -#endif // !_DOXYGEN_ +# endif // !_DOXYGEN_ + /* ****************************************************************************************************************** */ -} // namespace ros1 } // namespace ros2 +} // namespace common } // namespace fpsdk -#endif // __FPSDK_ROS2_ROS1_HPP__ +#endif // FPSDK_USE_ROS2 || _DOXYGEN_ +#endif // __FPSDK_COMMON_ROS2_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/trafo.hpp b/fpsdk_common/include/fpsdk_common/trafo.hpp index 70800ed1..3389a43c 100644 --- a/fpsdk_common/include/fpsdk_common/trafo.hpp +++ b/fpsdk_common/include/fpsdk_common/trafo.hpp @@ -210,9 +210,11 @@ Eigen::Vector3d LlhDegToRad(const Eigen::Vector3d& llh_deg); */ Eigen::Vector3d LlhRadToDeg(const Eigen::Vector3d& llh_rad); -#if FPSDK_USE_PROJ +#if FPSDK_USE_PROJ || defined(_DOXYGEN_) /** * @brief "Universal" coordinate transformer, backed by PROJ + * + * @note This is only available if compiled with PROJ, see @ref FPSDK_BUILD_DEPS. */ class Transformer : private types::NoCopyNoMove { diff --git a/fpsdk_common/include/fpsdk_common/video.hpp b/fpsdk_common/include/fpsdk_common/video.hpp index b30caf02..14c6eebb 100644 --- a/fpsdk_common/include/fpsdk_common/video.hpp +++ b/fpsdk_common/include/fpsdk_common/video.hpp @@ -14,6 +14,7 @@ * * **API**: fpsdk_common/video.hpp and fpsdk::common::video * + * @note This is only available if compiled with FFmpeg, see @ref FPSDK_BUILD_DEPS. */ #ifndef __FPSDK_COMMON_VIDEO_HPP__ #define __FPSDK_COMMON_VIDEO_HPP__ @@ -35,7 +36,7 @@ namespace common { */ namespace video { /* ****************************************************************************************************************** */ -#if FPSDK_USE_FFMPEG +#if FPSDK_USE_FFMPEG || defined(_DOXYGEN_) /** * @brief Video codec @@ -191,7 +192,7 @@ struct ImageData /** * @brief Helper for decoding video frames * - * Note that separate instances of this should be used to process different video streams. + * @note This is only available if compiled with FFmpeg, see @ref FPSDK_BUILD_DEPS. */ class VideoFrameDecoder { @@ -224,6 +225,7 @@ class VideoFrameDecoder * types 16-23 (IRAP, Intra Random Access Point) NALUs might work) * - The video encoder should be configured accordingly (to repeat the VPS/SPS/PPS for each I-frame, and possibly to * produce all/only I frames. + * - Separate instances of this should be used to process different video streams * * @param[in] data Data for one (not more, not less) video frame * @param[in] size Size of data diff --git a/fpsdk_common/rosnoros/ros/time.cpp b/fpsdk_common/rosnoros/ros/time.cpp new file mode 100644 index 00000000..87f1de4c --- /dev/null +++ b/fpsdk_common/rosnoros/ros/time.cpp @@ -0,0 +1,331 @@ +/* + * Copyright (C) 2009, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @file + * @brief ROS time polyfill (implementation) + * + * @details Files impoorted: + * - ros/src/duration.cpp + * - ros/src/time.cpp + * + * @details Unlike the real thing there is no simulation time here. ros::Time::now() always is the wall clock time and + * Time::init()/shutdown()/setNow() do nothing. The sleep(), toBoost() and fromBoost() methods and the Rate + * class are not implemented (they are declared in time.h, but never defined). This file is only compiled in + * the non-ROS1 case (see ../../CMakeLists.txt), where the real librostime is not available. + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#include +#include +#include +#include +#include + +#include "time.h" + +namespace ros { + +/**********************************************************************************************************************/ +// ros/src/duration.cpp +/**********************************************************************************************************************/ + +void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec) { + int64_t nsec_part = nsec % 1000000000L; + int64_t sec_part = sec + (nsec / 1000000000L); + if (nsec_part < 0) { + nsec_part += 1000000000L; + --sec_part; + } + if ((sec_part < std::numeric_limits::min()) || (sec_part > std::numeric_limits::max())) { + throw std::runtime_error("Duration is out of dual 32-bit range"); + } + sec = sec_part; + nsec = nsec_part; +} + +void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec) { + int64_t sec64 = sec; + int64_t nsec64 = nsec; + normalizeSecNSecSigned(sec64, nsec64); + sec = static_cast(sec64); + nsec = static_cast(nsec64); +} + +/**********************************************************************************************************************/ +// ros/src/time.cpp +/**********************************************************************************************************************/ + +void normalizeSecNSec(uint64_t& sec, uint64_t& nsec) { + const uint64_t nsec_part = nsec % 1000000000UL; + const uint64_t sec_part = nsec / 1000000000UL; + if ((sec + sec_part) > std::numeric_limits::max()) { + throw std::runtime_error("Time is out of dual 32-bit range"); + } + sec += sec_part; + nsec = nsec_part; +} + +void normalizeSecNSec(uint32_t& sec, uint32_t& nsec) { + uint64_t sec64 = sec; + uint64_t nsec64 = nsec; + normalizeSecNSec(sec64, nsec64); + sec = static_cast(sec64); + nsec = static_cast(nsec64); +} + +void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec) { + int64_t nsec_part = nsec % 1000000000L; + int64_t sec_part = sec + (nsec / 1000000000L); + if (nsec_part < 0) { + nsec_part += 1000000000L; + --sec_part; + } + if ((sec_part < 0) || (sec_part > static_cast(std::numeric_limits::max()))) { + throw std::runtime_error("Time is out of dual 32-bit range"); + } + sec = sec_part; + nsec = nsec_part; +} + +// --------------------------------------------------------------------------------------------------------------------- + +// Note: the order of these definitions matters (static initialisation order within this translation unit) + +const Duration DURATION_MAX(std::numeric_limits::max(), 999999999); +const Duration DURATION_MIN(std::numeric_limits::min(), 0); + +template <> +const Duration DurationBase::MAX = DURATION_MAX; +template <> +const Duration DurationBase::MIN = DURATION_MIN; +template <> +const Duration DurationBase::ZERO = Duration(0, 0); +template <> +const Duration DurationBase::NANOSECOND = Duration(0, 1); +template <> +const Duration DurationBase::MICROSECOND = Duration(0, 1000); +template <> +const Duration DurationBase::MILLISECOND = Duration(0, 1000000); +template <> +const Duration DurationBase::SECOND = Duration(1, 0); +template <> +const Duration DurationBase::MINUTE = Duration(60, 0); +template <> +const Duration DurationBase::HOUR = Duration(60 * 60, 0); +template <> +const Duration DurationBase::DAY = Duration(60 * 60 * 24, 0); + +template <> +const WallDuration DurationBase::MAX = WallDuration(Duration::MAX.sec, Duration::MAX.nsec); +template <> +const WallDuration DurationBase::MIN = WallDuration(Duration::MIN.sec, Duration::MIN.nsec); +template <> +const WallDuration DurationBase::ZERO = WallDuration(Duration::ZERO.sec, Duration::ZERO.nsec); +template <> +const WallDuration DurationBase::DAY = WallDuration(Duration::DAY.sec, Duration::DAY.nsec); +template <> +const WallDuration DurationBase::HOUR = WallDuration(Duration::HOUR.sec, Duration::HOUR.nsec); +template <> +const WallDuration DurationBase::MINUTE = WallDuration(Duration::MINUTE.sec, Duration::MINUTE.nsec); +template <> +const WallDuration DurationBase::SECOND = WallDuration(Duration::SECOND.sec, Duration::SECOND.nsec); +template <> +const WallDuration DurationBase::MILLISECOND = + WallDuration(Duration::MILLISECOND.sec, Duration::MILLISECOND.nsec); +template <> +const WallDuration DurationBase::MICROSECOND = + WallDuration(Duration::MICROSECOND.sec, Duration::MICROSECOND.nsec); +template <> +const WallDuration DurationBase::NANOSECOND = + WallDuration(Duration::NANOSECOND.sec, Duration::NANOSECOND.nsec); + +const Time TIME_MAX(std::numeric_limits::max(), 999999999); +const Time TIME_MIN(0, 1); + +template <> +const Time TimeBase::MAX = TIME_MAX; +template <> +const Time TimeBase::MIN = TIME_MIN; +template <> +const Time TimeBase::ZERO = Time(0, 0); +template <> +const Time TimeBase::UNINITIALIZED = Time::ZERO; + +template <> +const WallTime TimeBase::MAX = WallTime(Time::MAX.sec, Time::MAX.nsec); +template <> +const WallTime TimeBase::MIN = WallTime(Time::MIN.sec, Time::MIN.nsec); +template <> +const WallTime TimeBase::ZERO = WallTime(Time::ZERO.sec, Time::ZERO.nsec); +template <> +const WallTime TimeBase::UNINITIALIZED = + WallTime(Time::UNINITIALIZED.sec, Time::UNINITIALIZED.nsec); + +template <> +const SteadyTime TimeBase::MAX = SteadyTime(Time::MAX.sec, Time::MAX.nsec); +template <> +const SteadyTime TimeBase::MIN = SteadyTime(Time::MIN.sec, Time::MIN.nsec); +template <> +const SteadyTime TimeBase::ZERO = SteadyTime(Time::ZERO.sec, Time::ZERO.nsec); +template <> +const SteadyTime TimeBase::UNINITIALIZED = + SteadyTime(Time::UNINITIALIZED.sec, Time::UNINITIALIZED.nsec); + +// --------------------------------------------------------------------------------------------------------------------- + +void ros_walltime(uint32_t& sec, uint32_t& nsec) { + struct timespec start; + clock_gettime(CLOCK_REALTIME, &start); + if ((start.tv_sec < 0) || (start.tv_sec > static_cast(std::numeric_limits::max()))) { + throw std::runtime_error("Timespec is out of dual 32-bit range"); + } + sec = static_cast(start.tv_sec); + nsec = static_cast(start.tv_nsec); +} + +void ros_steadytime(uint32_t& sec, uint32_t& nsec) { + struct timespec start; + clock_gettime(CLOCK_MONOTONIC, &start); + sec = static_cast(start.tv_sec); + nsec = static_cast(start.tv_nsec); +} + +// --------------------------------------------------------------------------------------------------------------------- + +// There is no simulation time here, it's always the wall clock + +bool Time::useSystemTime() { + return true; +} + +bool Time::isSimTime() { + return false; +} + +bool Time::isSystemTime() { + return true; +} + +bool Time::isValid() { + return true; +} + +bool Time::waitForValid() { + return true; +} + +bool Time::waitForValid(const ros::WallDuration& timeout) { + (void)timeout; + return true; +} + +void Time::init() { +} + +void Time::shutdown() { +} + +void Time::setNow(const Time& new_now) { + (void)new_now; +} + +Time Time::now() { + Time t; + ros_walltime(t.sec, t.nsec); + return t; +} + +WallTime WallTime::now() { + WallTime t; + ros_walltime(t.sec, t.nsec); + return t; +} + +SteadyTime SteadyTime::now() { + SteadyTime t; + ros_steadytime(t.sec, t.nsec); + return t; +} + +// --------------------------------------------------------------------------------------------------------------------- + +namespace { +// Print sec.nsec, restoring the stream flags afterwards +template +std::ostream& printTime(std::ostream& os, const T& rhs) { + const std::ios_base::fmtflags flags = os.flags(); + const char fill = os.fill(); + os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec; + os.fill(fill); + os.flags(flags); + return os; +} + +template +std::ostream& printDuration(std::ostream& os, const T& rhs) { + const std::ios_base::fmtflags flags = os.flags(); + const char fill = os.fill(); + if ((rhs.sec >= 0) || (rhs.nsec == 0)) { + os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec; + } else { + os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') + << (1000000000 - rhs.nsec); + } + os.fill(fill); + os.flags(flags); + return os; +} +} // namespace + +std::ostream& operator<<(std::ostream& os, const Time& rhs) { + return printTime(os, rhs); +} + +std::ostream& operator<<(std::ostream& os, const WallTime& rhs) { + return printTime(os, rhs); +} + +std::ostream& operator<<(std::ostream& os, const SteadyTime& rhs) { + return printTime(os, rhs); +} + +std::ostream& operator<<(std::ostream& os, const Duration& rhs) { + return printDuration(os, rhs); +} + +std::ostream& operator<<(std::ostream& os, const WallDuration& rhs) { + return printDuration(os, rhs); +} + +} // namespace ros diff --git a/fpsdk_common/rosnoros/ros/time.h b/fpsdk_common/rosnoros/ros/time.h index ca18a423..16e6099b 100644 --- a/fpsdk_common/rosnoros/ros/time.h +++ b/fpsdk_common/rosnoros/ros/time.h @@ -29,9 +29,14 @@ * @file * @brief ROS time polyfill * - * @details Files impoorted: + * @details Files imported: * - ros/duration.h * - ros/time.h + * - ros/impl/duration.h + * - ros/impl/time.h + * + * @details The implementation of the non-template parts is in time.cpp (which is only built in the non-ROS1 case). + * The toBoost()/fromBoost() and sleep() methods as well as the Rate class are not available. * * ___ ___ * \ \ / / @@ -51,7 +56,10 @@ #include #include #include +#include #include +#include +#include namespace boost { namespace posix_time { @@ -439,7 +447,221 @@ ROSTIME_DECL std::ostream& operator<<(std::ostream& os, const SteadyTime& rhs); } // namespace ros /**********************************************************************************************************************/ -// ros/....h +// ros/impl/duration.h +/**********************************************************************************************************************/ +namespace ros { + +template +DurationBase::DurationBase(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) { + normalizeSecNSecSigned(sec, nsec); +} + +template +T& DurationBase::fromSec(double d) { + if (!std::isfinite(d)) { + throw std::runtime_error("Duration has to be finite."); + } + constexpr double minInt64AsDouble = static_cast(std::numeric_limits::min()); + constexpr double maxInt64AsDouble = static_cast(std::numeric_limits::max()); + if ((d <= minInt64AsDouble) || (d >= maxInt64AsDouble)) { + throw std::runtime_error("Duration is out of 64-bit integer range"); + } + const int64_t sec64 = static_cast(std::floor(d)); + if ((sec64 < std::numeric_limits::min()) || (sec64 > std::numeric_limits::max())) { + throw std::runtime_error("Duration is out of dual 32-bit range"); + } + sec = static_cast(sec64); + nsec = static_cast(std::round((d - static_cast(sec)) * 1e9)); + const int32_t rollover = nsec / 1000000000; + sec += rollover; + nsec %= 1000000000; + return *static_cast(this); +} + +template +T& DurationBase::fromNSec(int64_t t) { + const int64_t sec64 = t / 1000000000LL; + if ((sec64 < std::numeric_limits::min()) || (sec64 > std::numeric_limits::max())) { + throw std::runtime_error("Duration is out of dual 32-bit range"); + } + sec = static_cast(sec64); + nsec = static_cast(t % 1000000000LL); + normalizeSecNSecSigned(sec, nsec); + return *static_cast(this); +} + +template +T DurationBase::operator+(const T& rhs) const { + T t; + return t.fromNSec(toNSec() + rhs.toNSec()); +} + +template +T DurationBase::operator*(double scale) const { + return T(toSec() * scale); +} + +template +T DurationBase::operator-(const T& rhs) const { + T t; + return t.fromNSec(toNSec() - rhs.toNSec()); +} + +template +T DurationBase::operator-() const { + T t; + return t.fromNSec(-toNSec()); +} + +template +T& DurationBase::operator+=(const T& rhs) { + *this = *this + rhs; + return *static_cast(this); +} + +template +T& DurationBase::operator-=(const T& rhs) { + *this += (-rhs); + return *static_cast(this); +} + +template +T& DurationBase::operator*=(double scale) { + fromSec(toSec() * scale); + return *static_cast(this); +} + +template +bool DurationBase::operator<(const T& rhs) const { + return (sec < rhs.sec) || ((sec == rhs.sec) && (nsec < rhs.nsec)); +} + +template +bool DurationBase::operator>(const T& rhs) const { + return (sec > rhs.sec) || ((sec == rhs.sec) && (nsec > rhs.nsec)); +} + +template +bool DurationBase::operator<=(const T& rhs) const { + return (sec < rhs.sec) || ((sec == rhs.sec) && (nsec <= rhs.nsec)); +} + +template +bool DurationBase::operator>=(const T& rhs) const { + return (sec > rhs.sec) || ((sec == rhs.sec) && (nsec >= rhs.nsec)); +} + +template +bool DurationBase::operator==(const T& rhs) const { + return (sec == rhs.sec) && (nsec == rhs.nsec); +} + +template +bool DurationBase::isZero() const { + return (sec == 0) && (nsec == 0); +} + +} // namespace ros + +/**********************************************************************************************************************/ +// ros/impl/time.h /**********************************************************************************************************************/ +namespace ros { + +template +T& TimeBase::fromNSec(uint64_t t) { + uint64_t sec64 = 0; + uint64_t nsec64 = t; + normalizeSecNSec(sec64, nsec64); + sec = static_cast(sec64); + nsec = static_cast(nsec64); + return *static_cast(this); +} + +template +T& TimeBase::fromSec(double t) { + if (t < 0) { + throw std::runtime_error("Time cannot be negative."); + } + if (!std::isfinite(t)) { + throw std::runtime_error("Time has to be finite."); + } + constexpr double maxInt64AsDouble = static_cast(std::numeric_limits::max()); + if (t >= maxInt64AsDouble) { + throw std::runtime_error("Time is out of 64-bit integer range"); + } + const int64_t sec64 = static_cast(std::floor(t)); + if (sec64 > static_cast(std::numeric_limits::max())) { + throw std::runtime_error("Time is out of dual 32-bit range"); + } + sec = static_cast(sec64); + nsec = static_cast(std::round((t - static_cast(sec)) * 1e9)); + // avoid rounding errors + sec += (nsec / 1000000000u); + nsec %= 1000000000u; + return *static_cast(this); +} + +template +D TimeBase::operator-(const T& rhs) const { + D d; + return d.fromNSec(static_cast(toNSec()) - static_cast(rhs.toNSec())); +} + +template +T TimeBase::operator-(const D& rhs) const { + return *static_cast(this) + (-rhs); +} + +template +T TimeBase::operator+(const D& rhs) const { + int64_t sec_sum = static_cast(sec) + static_cast(rhs.sec); + int64_t nsec_sum = static_cast(nsec) + static_cast(rhs.nsec); + + // Throws an exception if we go out of 32-bit range + normalizeSecNSecUnsigned(sec_sum, nsec_sum); + + // now, it's safe to downcast back to uint32 bits + return T(static_cast(sec_sum), static_cast(nsec_sum)); +} + +template +T& TimeBase::operator+=(const D& rhs) { + *this = *this + rhs; + return *static_cast(this); +} + +template +T& TimeBase::operator-=(const D& rhs) { + *this += (-rhs); + return *static_cast(this); +} + +template +bool TimeBase::operator==(const T& rhs) const { + return (sec == rhs.sec) && (nsec == rhs.nsec); +} + +template +bool TimeBase::operator<(const T& rhs) const { + return (sec < rhs.sec) || ((sec == rhs.sec) && (nsec < rhs.nsec)); +} + +template +bool TimeBase::operator>(const T& rhs) const { + return (sec > rhs.sec) || ((sec == rhs.sec) && (nsec > rhs.nsec)); +} + +template +bool TimeBase::operator<=(const T& rhs) const { + return (sec < rhs.sec) || ((sec == rhs.sec) && (nsec <= rhs.nsec)); +} + +template +bool TimeBase::operator>=(const T& rhs) const { + return (sec > rhs.sec) || ((sec == rhs.sec) && (nsec >= rhs.nsec)); +} + +} // namespace ros #endif // __ROSNOROS__TIME_H__ diff --git a/fpsdk_common/src/app.cpp b/fpsdk_common/src/app.cpp index 9993cf19..8eb8c586 100644 --- a/fpsdk_common/src/app.cpp +++ b/fpsdk_common/src/app.cpp @@ -368,9 +368,9 @@ static constexpr const char* BUILDSTR = "debug"; #endif static constexpr const char* ROSSTR = -#if defined(FPSDK_USE_ROS1) +#if FPSDK_USE_ROS1 ", ROS1"; -#elif defined(FPSDK_USE_ROS2) +#elif FPSDK_USE_ROS2 ", ROS2"; #else ", noros"; @@ -387,19 +387,25 @@ static constexpr const char* FFMPEGSTR = #else ", noffmpeg"; #endif +static constexpr const char* BZ2STR = +#if FPSDK_USE_BZ2 + ", bz2"; +#else + ", nobz2"; +#endif void ProgramOptions::PrintVersion() const { - std::fprintf(stdout, "%s%s%s (fpsdk: %s%s%s%s, %s)\n%s\n%s\n", app_name_.c_str(), version_str_.empty() ? "" : " ", - version_str_.empty() ? "" : version_str_.c_str(), BUILDSTR, ROSSTR, PROJSTR, FFMPEGSTR, + std::fprintf(stdout, "%s%s%s (fpsdk: %s%s%s%s%s, %s)\n%s\n%s\n", app_name_.c_str(), version_str_.empty() ? "" : " ", + version_str_.empty() ? "" : version_str_.c_str(), BUILDSTR, ROSSTR, PROJSTR, FFMPEGSTR, BZ2STR, utils::GetVersionString(), copy_str_.empty() ? utils::GetCopyrightString() : copy_str_.c_str(), lic_str_.empty() ? utils::GetLicenseString() : lic_str_.c_str()); } void ProgramOptions::LogVersion() const { - INFO("%s%s%s (fpsdk: %s%s%s%s, %s)\n%s\n%s\n", app_name_.c_str(), version_str_.empty() ? "" : " ", - version_str_.empty() ? "" : version_str_.c_str(), BUILDSTR, ROSSTR, PROJSTR, FFMPEGSTR, + INFO("%s%s%s (fpsdk: %s%s%s%s%s, %s)\n%s\n%s\n", app_name_.c_str(), version_str_.empty() ? "" : " ", + version_str_.empty() ? "" : version_str_.c_str(), BUILDSTR, ROSSTR, PROJSTR, FFMPEGSTR, BZ2STR, utils::GetVersionString(), copy_str_.empty() ? utils::GetCopyrightString() : copy_str_.c_str(), lic_str_.empty() ? utils::GetLicenseString() : lic_str_.c_str()); } @@ -505,11 +511,9 @@ bool SystemdNotifyReady() return true; } - // We run systemd-notify(1) instead of using the sd_notify() API, as the latter would add the libsystemd dependency - // to nearly *everything* we build, which isn't desirable. And we also want to prevent complicated build - // configurations to make that dependency optional. And since we only need to do this notification once on start of - // every node, calling an external program seems a reasonable practical solution. Of course, if we wanted to use the - // full potential of sd_notify(), we'd just have to live with the libsystemd dependency. + // We run systemd-notify(1) instead of using the sd_notify() API, as the latter would add the libsystemd dependency, + // which isn't desirable. Calling an external program seems a reasonable practical solution. Of course, if we wanted + // to use the full potential of sd_notify(), we'd just have to live with the libsystemd dependency. INFO("SystemdNotifyReady"); // Note that std::system() runs the command through the command processor (/bin/sh). So we have to "exec" so that // systemd-notify becomes a direct child process (which is a requirement for --ready to work, see the man page). @@ -517,8 +521,6 @@ bool SystemdNotifyReady() if (res != 0) { WARNING("SystemdNotifyReady fail (%d)", res); return false; - } else { - return true; } return true; diff --git a/fpsdk_common/src/ros1.cpp b/fpsdk_common/src/ros1.cpp index 5381f7a2..1b668a6a 100644 --- a/fpsdk_common/src/ros1.cpp +++ b/fpsdk_common/src/ros1.cpp @@ -5,6 +5,8 @@ * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors * / /\ \ License: see the LICENSE file * /__/ \__\ + * + * Parts copyright (c) 2008, Willow Garage, Inc. * \endverbatim * * @file @@ -12,8 +14,17 @@ */ /* LIBC/STL */ +#include +#include +#include +#include +#include +#include /* EXTERNAL */ +#if FPSDK_USE_BZ2 +# include +#endif /* PACKAGE */ #include "fpsdk_common/logging.hpp" @@ -24,8 +35,611 @@ namespace common { namespace ros1 { /* ****************************************************************************************************************** */ +ros::Time ConvTime(const time::Time& time) +{ + const auto rt = time.GetRosTime(); + return ros::Time(rt.sec_, rt.nsec_); +} + +time::Time ConvTime(const ros::Time& time) +{ + return time::Time::FromRosTime({ time.sec, time.nsec }); +} + +/* ****************************************************************************************************************** */ + +// Rosbag format version 2.0. See http://wiki.ros.org/Bags/Format/2.0 and, for the reference implementation, +// ros_comm/rosbag_storage in the ROS1 (Noetic) sources. +namespace { + +using HeaderFields = std::map; + +// Record "op" field values +static constexpr uint8_t OP_MSG_DATA = 0x02; +static constexpr uint8_t OP_FILE_HEADER = 0x03; +static constexpr uint8_t OP_INDEX_DATA = 0x04; +static constexpr uint8_t OP_CHUNK = 0x05; +static constexpr uint8_t OP_CHUNK_INFO = 0x06; +static constexpr uint8_t OP_CONNECTION = 0x07; + +static constexpr char VERSION_LINE[] = "#ROSBAG V2.0\n"; +static constexpr uint32_t FILE_HEADER_LENGTH = 4 * 1024; // Bytes reserved for the file header record +static constexpr uint32_t INDEX_VERSION = 1; +static constexpr uint32_t CHUNK_INFO_VERSION = 1; +static constexpr uint32_t CHUNK_THRESHOLD = 768 * 1024; // Same as rosbag's default +static constexpr char COMPRESSION_NONE[] = "none"; +static constexpr char COMPRESSION_BZ2[] = "bz2"; +// Same parameters as rosbag (see ros_comm/rosbag_storage/src/bz2_stream.cpp) +#if FPSDK_USE_BZ2 +static constexpr int BZ2_BLOCK_SIZE_100K = 9; +static constexpr int BZ2_WORK_FACTOR = 30; +static constexpr int BZ2_VERBOSITY = 0; +#endif + +// Header field values are raw little-endian data, not strings +template +static std::string HeaderVal(const T value) +{ + std::string str(sizeof(T), '\0'); + std::memcpy(&str[0], &value, sizeof(T)); + return str; +} + +static std::string HeaderTime(const time::RosTime& time) +{ + std::string str(2 * sizeof(uint32_t), '\0'); + std::memcpy(&str[0], &time.sec_, sizeof(uint32_t)); + std::memcpy(&str[sizeof(uint32_t)], &time.nsec_, sizeof(uint32_t)); + return str; +} + +static void AppendU32(std::vector& buf, const uint32_t value) +{ + const uint8_t* data = (const uint8_t*)&value; + buf.insert(buf.end(), data, data + sizeof(value)); +} + +static void AppendStr(std::vector& buf, const std::string& str) +{ + buf.insert(buf.end(), str.cbegin(), str.cend()); +} + +// A record header: length of all fields, followed by the fields ("=") +static void AppendHeader(std::vector& buf, const HeaderFields& fields) +{ + uint32_t total_len = 0; + for (const auto& field : fields) { + total_len += sizeof(uint32_t) + field.first.size() + 1 + field.second.size(); + } + AppendU32(buf, total_len); + for (const auto& field : fields) { + AppendU32(buf, field.first.size() + 1 + field.second.size()); + AppendStr(buf, field.first); + buf.push_back('='); + AppendStr(buf, field.second); + } +} + +// One entry in a chunk's per-connection index +struct IndexEntry +{ + IndexEntry(const time::RosTime& time, const uint32_t offset) + : sec_{ time.sec_ }, nsec_{ time.nsec_ }, offset_{ offset } + { + } + uint32_t sec_; + uint32_t nsec_; + uint32_t offset_; //!< Offset of the message record within the (uncompressed) chunk data + bool operator<(const IndexEntry& rhs) const + { + return (sec_ < rhs.sec_) || ((sec_ == rhs.sec_) && (nsec_ < rhs.nsec_)); + } +}; + +struct ChunkInfo +{ + uint64_t pos_ = 0; //!< Offset of the chunk record in the file + time::RosTime start_time_; + time::RosTime end_time_; + std::map conn_counts_; //!< Number of messages per connection in this chunk +}; + +struct Connection +{ + uint32_t id_ = 0; + HeaderFields header_; //!< The connection header (type, md5sum, message_definition, ...) +}; + +static bool TimeLess(const time::RosTime& lhs, const time::RosTime& rhs) +{ + return (lhs.sec_ < rhs.sec_) || ((lhs.sec_ == rhs.sec_) && (lhs.nsec_ < rhs.nsec_)); +} + +} // namespace + +// --------------------------------------------------------------------------------------------------------------------- + +struct BagWriter::Impl +{ + // Message definitions from .fpl (AddMsgDef()), by topic. These survive Open()/Close(). + std::map msg_defs_; + + std::string path_; + std::fstream file_; + bool error_ = false; + uint64_t file_header_pos_ = 0; + uint64_t index_data_pos_ = 0; + std::map conns_; // Connections, by topic + std::vector chunks_; + // The compression used for all chunks of this bag. Fixed at Open(), so that the placeholder chunk header and the + // final one have the same size (see WriteChunkHeaderRecord()). + std::string compression_ = COMPRESSION_NONE; + + // Current chunk + bool chunk_open_ = false; + ChunkInfo chunk_info_; + std::vector chunk_buf_; //!< The chunk data (records) is assembled in memory + std::vector comp_buf_; //!< The compressed chunk data (only used if compression_ != none) + std::map> chunk_index_; + + // ----------------------------------------------------------------------------------------------------------------- + + bool Write(const void* data, const std::size_t size) + { + if (!error_) { + file_.write((const char*)data, size); + if (!file_) { + WARNING("BagWriter: write fail %s", path_.c_str()); + error_ = true; + } + } + return !error_; + } + + bool Write(const std::vector& buf) + { + return Write(buf.data(), buf.size()); + } + + bool Seek(const uint64_t pos) + { + if (!error_) { + file_.seekp(pos, std::ios::beg); + if (!file_) { + WARNING("BagWriter: seek fail %s", path_.c_str()); + error_ = true; + } + } + return !error_; + } + + bool SeekEnd() + { + if (!error_) { + file_.seekp(0, std::ios::end); + if (!file_) { + WARNING("BagWriter: seek fail %s", path_.c_str()); + error_ = true; + } + } + return !error_; + } + + uint64_t Tell() + { + return (error_ ? 0 : (uint64_t)file_.tellp()); + } + + // ----------------------------------------------------------------------------------------------------------------- + + bool Open(const std::string& path, const std::string& compression) + { + Close(); + path_ = path; + compression_ = compression; + error_ = false; + conns_.clear(); + chunks_.clear(); + chunk_open_ = false; + chunk_buf_.clear(); + comp_buf_.clear(); + chunk_index_.clear(); + chunk_info_ = ChunkInfo(); + index_data_pos_ = 0; + + file_.open(path, std::ios::in | std::ios::out | std::ios::binary | std::ios::trunc); + if (!file_.is_open()) { + WARNING("BagWriter: open fail %s", path.c_str()); + error_ = true; + return false; + } + + // Version line, followed by the (for now empty) file header record + Write(VERSION_LINE, sizeof(VERSION_LINE) - 1); + file_header_pos_ = Tell(); + WriteFileHeaderRecord(); + + if (error_) { + file_.close(); + return false; + } + return true; + } + + void Close() + { + if (file_.is_open()) { + if (chunk_open_) { + StopWritingChunk(); + } + // The connection and chunk info records ("the index") go at the end of the file... + SeekEnd(); + index_data_pos_ = Tell(); + WriteConnectionRecords(); + WriteChunkInfoRecords(); + // ...and now we know what to put into the file header + Seek(file_header_pos_); + WriteFileHeaderRecord(); + file_.close(); + } + path_.clear(); + } + + // ----------------------------------------------------------------------------------------------------------------- + + void WriteFileHeaderRecord() + { + const uint32_t conn_count = conns_.size(); + const uint32_t chunk_count = chunks_.size(); + const HeaderFields fields = { // clang-format off + { "op", HeaderVal(OP_FILE_HEADER) }, + { "index_pos", HeaderVal(index_data_pos_) }, + { "conn_count", HeaderVal(conn_count) }, + { "chunk_count", HeaderVal(chunk_count) } }; // clang-format on + + // Note: the size of this record must not change, so that it can be rewritten in place on Close(). It is padded + // to a fixed size. All fields above have a fixed-size (binary) value, so the header length is constant, too. + std::vector buf; + AppendHeader(buf, fields); + const uint32_t header_len = buf.size() - sizeof(uint32_t); + const uint32_t data_len = (header_len < FILE_HEADER_LENGTH ? FILE_HEADER_LENGTH - header_len : 0); + AppendU32(buf, data_len); + buf.resize(buf.size() + data_len, ' '); + Write(buf); + } + + void WriteChunkHeaderRecord(const uint32_t compressed_size, const uint32_t uncompressed_size) + { + const HeaderFields fields = { // clang-format off + { "op", HeaderVal(OP_CHUNK) }, + { "compression", compression_ }, + { "size", HeaderVal(uncompressed_size) } }; // clang-format on + std::vector buf; + AppendHeader(buf, fields); + AppendU32(buf, compressed_size); + Write(buf); + } + + void StartWritingChunk(const time::RosTime& time) + { + chunk_info_ = ChunkInfo(); + chunk_info_.pos_ = Tell(); + chunk_info_.start_time_ = time; + chunk_info_.end_time_ = time; + chunk_buf_.clear(); + chunk_index_.clear(); + // Placeholder header, rewritten in StopWritingChunk() once the size is known + WriteChunkHeaderRecord(0, 0); + chunk_open_ = true; + } + + void StopWritingChunk() + { + const uint32_t uncompressed_size = chunk_buf_.size(); + uint32_t compressed_size = uncompressed_size; + if (compression_ == COMPRESSION_BZ2) { + if (CompressChunkBz2(compressed_size)) { + Write(comp_buf_.data(), compressed_size); + } + } else { + Write(chunk_buf_); + } + const uint64_t end_pos = Tell(); + + // Rewrite the chunk header, now that we know the sizes + Seek(chunk_info_.pos_); + WriteChunkHeaderRecord(compressed_size, uncompressed_size); + Seek(end_pos); + + WriteIndexRecords(); + + chunks_.push_back(chunk_info_); + chunk_open_ = false; + chunk_buf_.clear(); + chunk_index_.clear(); + } + + // Compress chunk_buf_ into comp_buf_. Returns false (and flags an error) on failure. Note that the compressed data + // can be larger than the uncompressed data (for incompressible data), same as in rosbag. + bool CompressChunkBz2(uint32_t& compressed_size) + { +#if FPSDK_USE_BZ2 + // Worst case output size for bzip2 is 1% + 600 bytes more than the input + unsigned int dest_len = chunk_buf_.size() + (chunk_buf_.size() / 100) + 600; + comp_buf_.resize(dest_len); + const int res = BZ2_bzBuffToBuffCompress((char*)comp_buf_.data(), &dest_len, (char*)chunk_buf_.data(), + chunk_buf_.size(), BZ2_BLOCK_SIZE_100K, BZ2_VERBOSITY, BZ2_WORK_FACTOR); + if (res != BZ_OK) { + WARNING("BagWriter: bz2 compress fail (%d) %s", res, path_.c_str()); + error_ = true; + return false; + } + compressed_size = dest_len; + return true; +#else + (void)compressed_size; + WARNING("BagWriter: no bz2 support"); + error_ = true; + return false; +#endif + } + + void WriteIndexRecords() + { + for (auto& entry : chunk_index_) { + const uint32_t conn_id = entry.first; + std::vector& index = entry.second; + // The index must be sorted by time (the messages are normally, but not necessarily, in time order) + std::stable_sort(index.begin(), index.end()); + + const uint32_t count = index.size(); + const HeaderFields fields = { // clang-format off + { "op", HeaderVal(OP_INDEX_DATA) }, + { "conn", HeaderVal(conn_id) }, + { "ver", HeaderVal(INDEX_VERSION) }, + { "count", HeaderVal(count) } }; // clang-format on + std::vector buf; + AppendHeader(buf, fields); + AppendU32(buf, count * 3 * sizeof(uint32_t)); + for (const auto& ix : index) { + AppendU32(buf, ix.sec_); + AppendU32(buf, ix.nsec_); + AppendU32(buf, ix.offset_); + } + Write(buf); + } + } + + void WriteConnectionRecords() + { + // In connection id order (conns_ is by topic), same as rosbag does it + std::vector::value_type*> conns; + conns.reserve(conns_.size()); + for (const auto& entry : conns_) { + conns.push_back(&entry); + } + std::sort(conns.begin(), conns.end(), + [](const auto* lhs, const auto* rhs) { return lhs->second.id_ < rhs->second.id_; }); + for (const auto* entry : conns) { + std::vector buf; + AppendConnectionRecord(buf, entry->first, entry->second); + Write(buf); + } + } + + static void AppendConnectionRecord(std::vector& buf, const std::string& topic, const Connection& conn) + { + const HeaderFields fields = { // clang-format off + { "op", HeaderVal(OP_CONNECTION) }, + { "topic", topic }, + { "conn", HeaderVal(conn.id_) } }; // clang-format on + AppendHeader(buf, fields); + AppendHeader(buf, conn.header_); + } + + void WriteChunkInfoRecords() + { + for (const auto& chunk : chunks_) { + const uint32_t count = chunk.conn_counts_.size(); + const HeaderFields fields = { // clang-format off + { "op", HeaderVal(OP_CHUNK_INFO) }, + { "ver", HeaderVal(CHUNK_INFO_VERSION) }, + { "chunk_pos", HeaderVal(chunk.pos_) }, + { "start_time", HeaderTime(chunk.start_time_) }, + { "end_time", HeaderTime(chunk.end_time_) }, + { "count", HeaderVal(count) } }; // clang-format on + std::vector buf; + AppendHeader(buf, fields); + AppendU32(buf, count * 2 * sizeof(uint32_t)); + for (const auto& entry : chunk.conn_counts_) { + AppendU32(buf, entry.first); + AppendU32(buf, entry.second); + } + Write(buf); + } + } + + // ----------------------------------------------------------------------------------------------------------------- + + bool WriteMessage(const std::string& topic, const time::RosTime& time, const std::string& msg_name, + const std::string& msg_md5, const std::string& msg_def, const uint8_t* data, const std::size_t size) + { + if (!file_.is_open()) { + WARNING("BagWriter: no bag open"); + return false; + } + if (error_) { + return false; + } + + // Find, or create, the connection for this topic + auto conn_entry = conns_.find(topic); + const bool new_conn = (conn_entry == conns_.end()); + if (new_conn) { + Connection conn; + conn.id_ = conns_.size(); + // Prefer the message definition from the .fpl (AddMsgDef()), fall back to the message type's traits + const auto msg_def_entry = msg_defs_.find(topic); + if (msg_def_entry != msg_defs_.end()) { + conn.header_ = msg_def_entry->second; + } else { + conn.header_.emplace("type", msg_name); + conn.header_.emplace("md5sum", msg_md5); + conn.header_.emplace("message_definition", msg_def); + } + conn_entry = conns_.emplace(topic, std::move(conn)).first; + } + const Connection& conn = conn_entry->second; + + if (!chunk_open_) { + StartWritingChunk(time); + } + + // A new connection is announced in the chunk (and again at the end of the file, see WriteConnectionRecords()) + if (new_conn) { + AppendConnectionRecord(chunk_buf_, topic, conn); + } + + // Index the message record that we're about to add to the chunk + chunk_index_[conn.id_].emplace_back(time, (uint32_t)chunk_buf_.size()); + chunk_info_.conn_counts_[conn.id_]++; + if (TimeLess(chunk_info_.end_time_, time)) { + chunk_info_.end_time_ = time; + } else if (TimeLess(time, chunk_info_.start_time_)) { + chunk_info_.start_time_ = time; + } + + // The message record + const HeaderFields fields = { // clang-format off + { "op", HeaderVal(OP_MSG_DATA) }, + { "conn", HeaderVal(conn.id_) }, + { "time", HeaderTime(time) } }; // clang-format on + AppendHeader(chunk_buf_, fields); + AppendU32(chunk_buf_, size); + chunk_buf_.insert(chunk_buf_.end(), data, data + size); + + if (chunk_buf_.size() > CHUNK_THRESHOLD) { + StopWritingChunk(); + } + + return !error_; + } +}; + +/* ****************************************************************************************************************** */ + +BagWriter::BagWriter() /* clang-format off */ : + impl_ { std::make_unique() } +{ +} + +BagWriter::~BagWriter() +{ + Close(); +} + +// --------------------------------------------------------------------------------------------------------------------- + +bool BagWriter::Open(const std::string& path, const int compress) +{ + // Note: unlike rosbag we have no LZ4. So compress = 1 gives bz2, too (and not lz4). + std::string compression = COMPRESSION_NONE; + if (compress > 0) { +#if FPSDK_USE_BZ2 + compression = COMPRESSION_BZ2; +#else + WARNING("BagWriter: cannot create compressed %s, bz2 support not compiled in", path.c_str()); + return false; +#endif + } + if (!impl_->Open(path, compression)) { + return false; + } + DEBUG("BagWriter: %s (%s)", path.c_str(), compression.c_str()); + return true; +} + // --------------------------------------------------------------------------------------------------------------------- +void BagWriter::Close() +{ + impl_->Close(); +} + +// --------------------------------------------------------------------------------------------------------------------- + +void BagWriter::AddMsgDef(const fpl::RosMsgDef& rosmsgdef) +{ + if (rosmsgdef.valid_ && (impl_->msg_defs_.find(rosmsgdef.topic_name_) == impl_->msg_defs_.end())) { + DEBUG("BagWriter: %s", rosmsgdef.info_.c_str()); + impl_->msg_defs_.emplace( + rosmsgdef.topic_name_, HeaderFields{ // clang-format off + { "message_definition", rosmsgdef.msg_def_ }, + { "topic", rosmsgdef.topic_name_ }, + { "md5sum", rosmsgdef.msg_md5_ }, + { "type", rosmsgdef.msg_name_ } }); // clang-format on + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +bool BagWriter::WriteMessage(const fpl::RosMsgBin& rosmsgbin) +{ + if (!rosmsgbin.valid_) { + return false; + } + // For ROS1 we can directly write the serialised data. We should already know the message meta data (see + // AddMsgDef() above). + if (impl_->msg_defs_.find(rosmsgbin.topic_name_) == impl_->msg_defs_.end()) { + WARNING("BagWriter: missing message definition for %s", rosmsgbin.topic_name_.c_str()); + return false; + } + return impl_->WriteMessage( + rosmsgbin.topic_name_, rosmsgbin.rec_time_, "", "", "", rosmsgbin.msg_data_.data(), rosmsgbin.msg_data_.size()); +} + +// --------------------------------------------------------------------------------------------------------------------- + +bool BagWriter::WriteSerialised(const std::string& topic, const time::RosTime& time, const std::string& msg_name, + const std::string& msg_md5, const std::string& msg_def, const uint8_t* data, const std::size_t size) +{ + return impl_->WriteMessage(topic, time, msg_name, msg_md5, msg_def, data, size); +} + +/* ****************************************************************************************************************** */ + +#if FPSDK_USE_ROS1 + +static char g_logger_name[100]; + +static void sLoggingFn(const logging::LoggingParams& /*params*/, const logging::LoggingLevel level, const char* str) +{ + switch (level) { // clang-format off + case logging::LoggingLevel::TRACE: ROS_LOG(ros::console::levels::Debug, g_logger_name, "%s", str); break; + case logging::LoggingLevel::DEBUG: ROS_LOG(ros::console::levels::Debug, g_logger_name, "%s", str); break; + case logging::LoggingLevel::INFO: ROS_LOG(ros::console::levels::Info, g_logger_name, "%s", str); break; + case logging::LoggingLevel::NOTICE: ROS_LOG(ros::console::levels::Info, g_logger_name, "%s", str); break; + case logging::LoggingLevel::WARNING: ROS_LOG(ros::console::levels::Warn, g_logger_name, "%s", str); break; + case logging::LoggingLevel::ERROR: ROS_LOG(ros::console::levels::Error, g_logger_name, "%s", str); break; + case logging::LoggingLevel::FATAL: ROS_LOG(ros::console::levels::Fatal, g_logger_name, "%s", str); break; + } // clang-format on +} + +void RedirectLoggingToRosConsole(const char* logger_name) +{ + logging::LoggingParams params = logging::LoggingGetParams(); + params.fn_ = sLoggingFn; + params.level_ = logging::LoggingLevel::TRACE; // We leave it up to ROS to decide what to print + // Set logger name. Note that ROSCONSOLE_DEFAULT_NAME here is the "ros.fpsdk_common" package name. However, when + // called from the app (node) then the default argument to logger_name is that package's ROSCONSOLE_DEFAULT_NAME, + // e.g. "ros.my_package" + std::snprintf( + g_logger_name, sizeof(g_logger_name), "%s", logger_name != NULL ? logger_name : ROSCONSOLE_DEFAULT_NAME); + LoggingSetParams(params); +} + +#endif // FPSDK_USE_ROS1 + /* ****************************************************************************************************************** */ } // namespace ros1 } // namespace common diff --git a/fpsdk_ros2/src/bagwriter.cpp b/fpsdk_common/src/ros2.cpp similarity index 59% rename from fpsdk_ros2/src/bagwriter.cpp rename to fpsdk_common/src/ros2.cpp index 94c032e3..40501047 100644 --- a/fpsdk_ros2/src/bagwriter.cpp +++ b/fpsdk_common/src/ros2.cpp @@ -5,31 +5,73 @@ * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors * / /\ \ License: see the LICENSE file * /__/ \__\ + * * \endverbatim * * @file - * @brief Fixposition SDK: ROS2 bag writer + * @brief Fixposition SDK: ROS2 types and utils + * + * @note This is only available if compiled with ROS2, see @ref FPSDK_BUILD_DEPS. */ +#if FPSDK_USE_ROS2 /* LIBC/STL */ -#include +# include /* EXTERNAL */ -#include -#include "fpsdk_ros2/ext/msgs.hpp" - -/* Fixposition SDK */ -#include -#include /* PACKAGE */ -#include "fpsdk_ros2/bagwriter.hpp" -#include "fpsdk_ros2/ros1.hpp" -#include "fpsdk_ros2/utils.hpp" +# include "fpsdk_common/logging.hpp" +# include "fpsdk_common/ros2.hpp" namespace fpsdk { +namespace common { namespace ros2 { -namespace bagwriter { +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common::logging; + +// --------------------------------------------------------------------------------------------------------------------- + +static std::unique_ptr g_logger; + +static void sLoggingFn(const LoggingParams& /*params*/, const LoggingLevel level, const char* str) +{ + // By default these will appear under the "fpsdk_common" logger + switch (level) { // clang-format off + case LoggingLevel::TRACE: RCLCPP_DEBUG((*g_logger), "%s", str); break; + case LoggingLevel::DEBUG: RCLCPP_DEBUG((*g_logger), "%s", str); break; + case LoggingLevel::INFO: RCLCPP_INFO( (*g_logger), "%s", str); break; + case LoggingLevel::NOTICE: RCLCPP_INFO( (*g_logger), "%s", str); break; + case LoggingLevel::WARNING: RCLCPP_WARN( (*g_logger), "%s", str); break; + case LoggingLevel::ERROR: RCLCPP_ERROR((*g_logger), "%s", str); break; + case LoggingLevel::FATAL: RCLCPP_FATAL((*g_logger), "%s", str); break; + } // clang-format on +} + +void RedirectLoggingToRosConsole(const char* logger_name) +{ + g_logger = std::make_unique(rclcpp::get_logger(logger_name)); + LoggingParams params = LoggingGetParams(); + params.fn_ = sLoggingFn; + params.level_ = LoggingLevel::TRACE; // We leave it up to ROS to decide what to print + LoggingSetParams(params); +} + +// --------------------------------------------------------------------------------------------------------------------- + +rclcpp::Time ConvTime(const fpsdk::common::time::Time& time, rcl_clock_type_t clock_type) +{ + const auto rt = time.GetRosTime(); + return rclcpp::Time(rt.sec_, rt.nsec_, clock_type); +} + +fpsdk::common::time::Time ConvTime(const rclcpp::Time& time) +{ + const uint64_t nsec = time.nanoseconds(); + return fpsdk::common::time::Time::FromRosTime( + { static_cast(nsec / 1000000000), static_cast(nsec % 1000000000) }); +} /* ****************************************************************************************************************** */ BagWriter::BagWriter() @@ -43,22 +85,22 @@ BagWriter::~BagWriter() // --------------------------------------------------------------------------------------------------------------------- -bool BagWriter::Open(const std::string& path, const int compress) +bool BagWriter::Open(const std::string& path, const bool mcap, const int compress) { Close(); bag_ = std::make_unique(); rosbag2_storage::StorageOptions opts; opts.uri = path; - if (compress > 0) { + if (mcap) { opts.storage_id = "mcap"; // apt install ros-${ROS_DISTRO}-rosbag2-storage-mcap - opts.storage_preset_profile = (compress > 1 ? "zstd_small" : "zstd_fast"); + opts.storage_preset_profile = (compress > 0 ? "zstd_small" : "zstd_fast"); } else { opts.storage_id = "sqlite3"; } try { bag_->open(opts); } catch (const std::exception& ex) { - WARNING("BagWriter: open fail %s: %s. Maybe %s storage plugin is not installed?", path.c_str(), ex.what(), + WARNING("BagWriter: open fail %s: %s. Maybe the %s storage plugin is not installed?", path.c_str(), ex.what(), opts.storage_id.c_str()); bag_.reset(); return false; @@ -92,13 +134,13 @@ void BagWriter::AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef) template inline bool WriteMessageEx( - const common::fpl::RosMsgDef& rosmsgdef, const common::fpl::RosMsgBin& rosmsgbin, ros2::bagwriter::BagWriter& bag) + const common::fpl::RosMsgDef& rosmsgdef, const common::fpl::RosMsgBin& rosmsgbin, BagWriter& bag) { if (rosmsgdef.msg_name_ == ros::message_traits::datatype()) { Ros1MsgT ros1; common::ros1::DeserializeMessage(rosmsgbin.msg_data_, ros1); Ros2MsgT ros2; - ros2::ros1::Ros1ToRos2(ros1, ros2); + Ros1ToRos2(ros1, ros2); bag.WriteMessage(ros2, rosmsgbin.topic_name_, rosmsgbin.rec_time_); return true; } else { @@ -143,6 +185,7 @@ bool BagWriter::WriteMessage(const common::fpl::RosMsgBin& rosmsgbin) } /* ****************************************************************************************************************** */ -} // namespace bagwriter } // namespace ros2 +} // namespace common } // namespace fpsdk +#endif // FPSDK_USE_ROS2 diff --git a/fpsdk_common/test/ros1_test.cpp b/fpsdk_common/test/ros1_test.cpp index c2a4f779..ffb057b1 100644 --- a/fpsdk_common/test/ros1_test.cpp +++ b/fpsdk_common/test/ros1_test.cpp @@ -12,9 +12,19 @@ */ /* LIBC/STL */ +#include +#include +#include +#include +#include +#include +#include /* EXTERNAL */ #include +#if FPSDK_USE_BZ2 +# include +#endif /* PACKAGE */ #include @@ -23,11 +33,732 @@ namespace { /* ****************************************************************************************************************** */ using namespace fpsdk::common::ros1; +using namespace fpsdk::common::time; -TEST(Ros1, dummy) +// --------------------------------------------------------------------------------------------------------------------- + +TEST(Ros1Test, ConvTime) +{ + const ros::Time ros_time(1723650283, 125000000); + DEBUG("ros::Time %" PRIu32 " %" PRIu32, ros_time.sec, ros_time.nsec); + + const fpsdk::common::time::Time sdk_time = ConvTime(ros_time); + DEBUG("SDK Time %" PRIu32 " %" PRIu32, sdk_time.sec_, sdk_time.nsec_); + EXPECT_EQ(sdk_time.sec_, ros_time.sec + 27); // 1723650310 + EXPECT_EQ(sdk_time.nsec_, ros_time.nsec); + + const ros::Time ros_time_again = ConvTime(sdk_time); + DEBUG("ros::Time %" PRIu32 " %" PRIu32, ros_time_again.sec, ros_time_again.nsec); + + EXPECT_EQ(ros_time_again.sec, ros_time.sec); + EXPECT_EQ(ros_time_again.nsec, ros_time.nsec); +} + +// --------------------------------------------------------------------------------------------------------------------- + +// The ros::Time implementation from rosnoros/ros/time.cpp (or from the real ROS1) + +TEST(Ros1Test, RosTime) +{ + ros::Time t1(1, 999999999); + EXPECT_EQ(t1.sec, 1u); + EXPECT_EQ(t1.nsec, 999999999u); + EXPECT_EQ(t1.toNSec(), 1999999999u); + + // Normalisation of the nsec overflow + ros::Time t2(1, 2500000000); + EXPECT_EQ(t2.sec, 3u); + EXPECT_EQ(t2.nsec, 500000000u); + + ros::Time t3; + EXPECT_TRUE(t3.isZero()); + t3.fromNSec(1234000005678u); + EXPECT_EQ(t3.sec, 1234u); + EXPECT_EQ(t3.nsec, 5678u); + t3.fromSec(1234.5); + EXPECT_EQ(t3.sec, 1234u); + EXPECT_EQ(t3.nsec, 500000000u); + + // Comparison and arithmetic + EXPECT_TRUE(t1 < t2); + EXPECT_TRUE(t2 > t1); + EXPECT_TRUE(t1 != t2); + EXPECT_TRUE(t1 == ros::Time(1, 999999999)); + EXPECT_EQ((t2 - t1).toNSec(), 1500000001); + EXPECT_TRUE((t1 + ros::Duration(0, 1)) == ros::Time(2, 0)); + + // The constants + EXPECT_TRUE(ros::TIME_MIN == ros::Time(0, 1)); + EXPECT_TRUE(ros::Time::MIN == ros::TIME_MIN); + EXPECT_TRUE(ros::Time::ZERO.isZero()); + EXPECT_TRUE(ros::Duration::ZERO.isZero()); + EXPECT_EQ(ros::Duration::SECOND.toNSec(), 1000000000); + + // Now() should be a sensible wall clock time (> 2020-01-01, < 2100-01-01) + const ros::Time now = ros::Time::now(); + EXPECT_GT(now.sec, 1577836800u); + EXPECT_LT(now.sec, 4102444800u); +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST(Ros1Test, SerialiseDeserialise) { - EXPECT_TRUE(true); + sensor_msgs::Imu msg; + msg.header.seq = 42; + msg.header.stamp.fromNSec(1234000005678u); + msg.header.frame_id = "test_frame"; + msg.linear_acceleration.x = 1.25; + msg.angular_velocity.z = -2.5; + msg.orientation.w = 1.0; + + const uint32_t size = ros::serialization::serializationLength(msg); + EXPECT_GT(size, 0u); + std::vector buf(size); + ros::serialization::OStream stream(buf.data(), size); + ros::serialization::serialize(stream, msg); + + sensor_msgs::Imu msg2; + DeserializeMessage(buf, msg2); + EXPECT_EQ(msg2.header.seq, 42u); + EXPECT_EQ(msg2.header.stamp.sec, 1234u); + EXPECT_EQ(msg2.header.stamp.nsec, 5678u); + EXPECT_EQ(msg2.header.frame_id, "test_frame"); + EXPECT_EQ(msg2.linear_acceleration.x, 1.25); + EXPECT_EQ(msg2.angular_velocity.z, -2.5); + EXPECT_EQ(msg2.orientation.w, 1.0); +} + +/* ****************************************************************************************************************** */ +// A minimal, independent rosbag v2.0 reader, to check what BagWriter produces. This deliberately does not share any +// code with the writer. + +struct BagRecord +{ + std::map header_; + std::string data_; + uint8_t Op() const + { + const auto entry = header_.find("op"); + return (entry == header_.end() || entry->second.empty() ? 0xff : (uint8_t)entry->second[0]); + } + uint32_t U32(const std::string& field) const + { + uint32_t value = 0; + const auto entry = header_.find(field); + if ((entry != header_.end()) && (entry->second.size() == sizeof(value))) { + std::memcpy(&value, entry->second.data(), sizeof(value)); + } + return value; + } + uint64_t U64(const std::string& field) const + { + uint64_t value = 0; + const auto entry = header_.find(field); + if ((entry != header_.end()) && (entry->second.size() == sizeof(value))) { + std::memcpy(&value, entry->second.data(), sizeof(value)); + } + return value; + } + std::string Str(const std::string& field) const + { + const auto entry = header_.find(field); + return (entry == header_.end() ? std::string() : entry->second); + } +}; + +class BagReader +{ + public: + bool Load(const std::string& path) + { + std::ifstream file(path, std::ios::binary); + if (!file.is_open()) { + return false; + } + buf_.assign(std::istreambuf_iterator(file), std::istreambuf_iterator()); + return true; + } + + // Parse a "=..." header blob + static bool ParseHeader(const std::string& blob, std::map& fields) + { + std::size_t pos = 0; + while (pos < blob.size()) { + if ((blob.size() - pos) < 4) { + return false; + } + uint32_t len = 0; + std::memcpy(&len, &blob[pos], 4); + pos += 4; + if ((blob.size() - pos) < len) { + return false; + } + const std::string field = blob.substr(pos, len); + pos += len; + const std::size_t eq = field.find('='); + if (eq == std::string::npos) { + return false; + } + fields[field.substr(0, eq)] = field.substr(eq + 1); + } + return pos == blob.size(); + } + + // Read one "
" record + static bool ReadRecord(const std::string& buf, std::size_t& pos, BagRecord& rec) + { + if ((buf.size() - pos) < 4) { + return false; + } + uint32_t header_len = 0; + std::memcpy(&header_len, &buf[pos], 4); + pos += 4; + if ((buf.size() - pos) < header_len) { + return false; + } + if (!ParseHeader(buf.substr(pos, header_len), rec.header_)) { + return false; + } + pos += header_len; + if ((buf.size() - pos) < 4) { + return false; + } + uint32_t data_len = 0; + std::memcpy(&data_len, &buf[pos], 4); + pos += 4; + if ((buf.size() - pos) < data_len) { + return false; + } + rec.data_ = buf.substr(pos, data_len); + pos += data_len; + return true; + } + + std::string buf_; +}; + +// The parsed contents of a bag +struct BagContents +{ + uint32_t conn_count_ = 0; + uint32_t chunk_count_ = 0; + uint32_t n_chunks_ = 0; + uint32_t n_index_ = 0; + std::string compression_; //!< The compression of the last chunk seen + uint64_t compressed_ = 0; //!< Total compressed size of all chunks + uint64_t uncompressed_ = 0; //!< Total uncompressed size of all chunks + std::map topics_; //!< Topic by connection id + std::map> conn_hdrs_; //!< Connection header by connection id + std::map> tail_hdrs_; //!< Ditto, from the index section + std::map> msgs_; //!< Message data by connection id + std::map> times_; //!< Message times by connection id + std::map info_counts_; //!< Counts from the CHUNK_INFO records +}; + +// Read and check a bag. Returns "" on success, or a description of the problem. +std::string ReadBag(const std::string& path, BagContents& bag) +{ +#define BAGCHECK(cond, msg) \ + if (!(cond)) { \ + return (msg); \ + } + BagReader reader; + BAGCHECK(reader.Load(path), "cannot open"); + const std::string& buf = reader.buf_; + BAGCHECK(buf.substr(0, 13) == "#ROSBAG V2.0\n", "bad version line"); + + // File header record + std::size_t pos = 13; + BagRecord fh; + BAGCHECK(BagReader::ReadRecord(buf, pos, fh), "no file header record"); + BAGCHECK(fh.Op() == 0x03, "no FILE_HEADER op"); + BAGCHECK(pos == (13 + 4096 + 8), "file header record not padded to 4096"); + const uint64_t index_pos = fh.U64("index_pos"); + bag.conn_count_ = fh.U32("conn_count"); + bag.chunk_count_ = fh.U32("chunk_count"); + BAGCHECK((index_pos >= pos) && (index_pos <= buf.size()), "bad index_pos"); // == pos for an empty bag + + // Chunk section + std::vector chunk_offsets; + while (pos < index_pos) { + BagRecord rec; + BAGCHECK(BagReader::ReadRecord(buf, pos, rec), "bad record in chunk section"); + // Chunk + if (rec.Op() == 0x05) { + bag.n_chunks_++; + bag.compression_ = rec.Str("compression"); + const uint32_t uncompressed_size = rec.U32("size"); + bag.compressed_ += rec.data_.size(); + bag.uncompressed_ += uncompressed_size; + if (bag.compression_ == "none") { + BAGCHECK(uncompressed_size == rec.data_.size(), "chunk size mismatch"); + } else if (bag.compression_ == "bz2") { +#if FPSDK_USE_BZ2 + // Decompress exactly the way rosbag's Bag::decompressBz2Chunk() does it + std::string plain(uncompressed_size, '\0'); + unsigned int dest_len = uncompressed_size; + const int res = BZ2_bzBuffToBuffDecompress(uncompressed_size > 0 ? &plain[0] : nullptr, &dest_len, + rec.data_.empty() ? nullptr : &rec.data_[0], rec.data_.size(), 0, 0); + BAGCHECK(res == BZ_OK, "bz2 decompress fail"); + BAGCHECK(dest_len == uncompressed_size, "bz2 chunk size mismatch"); + rec.data_ = plain; +#else + return "bz2 chunk but no bz2 support"; +#endif + } else { + return "unexpected compression"; + } + chunk_offsets.clear(); + std::size_t cpos = 0; + while (cpos < rec.data_.size()) { + chunk_offsets.push_back(cpos); + BagRecord crec; + BAGCHECK(BagReader::ReadRecord(rec.data_, cpos, crec), "bad record in chunk"); + // Connection + if (crec.Op() == 0x07) { + const uint32_t id = crec.U32("conn"); + BAGCHECK(bag.topics_.count(id) == 0, "duplicate connection id"); + bag.topics_[id] = crec.Str("topic"); + BAGCHECK(BagReader::ParseHeader(crec.data_, bag.conn_hdrs_[id]), "bad connection header"); + } + // Message + else if (crec.Op() == 0x02) { + const uint32_t id = crec.U32("conn"); + const std::string time = crec.Str("time"); + BAGCHECK(time.size() == 8, "bad message time field"); + uint32_t sec = 0; + uint32_t nsec = 0; + std::memcpy(&sec, &time[0], 4); + std::memcpy(&nsec, &time[4], 4); + bag.msgs_[id].push_back(crec.data_); + bag.times_[id].push_back(RosTime(sec, nsec)); + } else { + return "unexpected op in chunk"; + } + } + BAGCHECK(cpos == rec.data_.size(), "chunk records overrun"); + } + // Index + else if (rec.Op() == 0x04) { + bag.n_index_++; + BAGCHECK(rec.U32("ver") == 1, "bad index version"); + const uint32_t count = rec.U32("count"); + BAGCHECK(rec.data_.size() == (count * 12), "index data size mismatch"); + uint64_t prev = 0; + for (uint32_t ix = 0; ix < count; ix++) { + uint32_t sec = 0; + uint32_t nsec = 0; + uint32_t offset = 0; + std::memcpy(&sec, &rec.data_[(ix * 12) + 0], 4); + std::memcpy(&nsec, &rec.data_[(ix * 12) + 4], 4); + std::memcpy(&offset, &rec.data_[(ix * 12) + 8], 4); + const uint64_t time = ((uint64_t)sec * 1000000000) + nsec; + BAGCHECK(time >= prev, "index not sorted by time"); + prev = time; + bool found = false; + for (const auto cand : chunk_offsets) { + if (cand == offset) { + found = true; + break; + } + } + BAGCHECK(found, "index offset is not a record boundary"); + } + } else { + return "unexpected op in chunk section"; + } + } + BAGCHECK(pos == index_pos, "chunk section does not end at index_pos"); + BAGCHECK(bag.n_chunks_ == bag.chunk_count_, "chunk count mismatch"); + + // Index section + uint32_t n_conn = 0; + uint32_t n_info = 0; + uint32_t prev_conn_id = 0; + while (pos < buf.size()) { + BagRecord rec; + BAGCHECK(BagReader::ReadRecord(buf, pos, rec), "bad record in index section"); + // Connection + if (rec.Op() == 0x07) { + const uint32_t id = rec.U32("conn"); + BAGCHECK((n_conn == 0) || (id > prev_conn_id), "connection records not in id order"); + prev_conn_id = id; + n_conn++; + BAGCHECK(BagReader::ParseHeader(rec.data_, bag.tail_hdrs_[id]), "bad connection header"); + } + // Chunk info + else if (rec.Op() == 0x06) { + n_info++; + BAGCHECK(rec.U32("ver") == 1, "bad chunk info version"); + BAGCHECK(rec.Str("start_time").size() == 8, "bad start_time"); + BAGCHECK(rec.Str("end_time").size() == 8, "bad end_time"); + const uint32_t count = rec.U32("count"); + BAGCHECK(rec.data_.size() == (count * 8), "chunk info data size mismatch"); + for (uint32_t ix = 0; ix < count; ix++) { + uint32_t id = 0; + uint32_t num = 0; + std::memcpy(&id, &rec.data_[(ix * 8) + 0], 4); + std::memcpy(&num, &rec.data_[(ix * 8) + 4], 4); + bag.info_counts_[id] += num; + } + } else { + return "unexpected op in index section"; + } + } + BAGCHECK(pos == buf.size(), "trailing garbage"); + BAGCHECK(n_conn == bag.conn_count_, "connection count mismatch"); + BAGCHECK(n_info == bag.chunk_count_, "chunk info count mismatch"); + BAGCHECK(bag.tail_hdrs_ == bag.conn_hdrs_, "index connection headers differ from the in-chunk ones"); + for (const auto& entry : bag.msgs_) { + BAGCHECK(bag.info_counts_[entry.first] == entry.second.size(), "CHUNK_INFO counts != actual messages"); + } +#undef BAGCHECK + return ""; +} + +// --------------------------------------------------------------------------------------------------------------------- + +class BagWriterTest : public ::testing::Test +{ + protected: + void SetUp() override + { + char tmpl[] = "/tmp/fpsdk_bagwriter_test_XXXXXX"; + const int fd = mkstemp(tmpl); + ASSERT_GE(fd, 0); + close(fd); + path_ = tmpl; + } + void TearDown() override + { + std::remove(path_.c_str()); + } + std::string path_; +}; + +// --------------------------------------------------------------------------------------------------------------------- + +TEST_F(BagWriterTest, WriteAndReadBack) +{ + const RosTime t0(1750000000, 500000); + + { + BagWriter bag; + ASSERT_TRUE(bag.Open(path_)); + + for (int ix = 0; ix < 10; ix++) { + sensor_msgs::Imu imu; + imu.header.seq = ix; + imu.header.frame_id = "imu_frame"; + imu.header.stamp = ros::Time(t0.sec_ + ix, t0.nsec_); + imu.linear_acceleration.x = ix; + EXPECT_TRUE(bag.WriteMessage(imu, "/imu/data", RosTime(t0.sec_ + ix, t0.nsec_))); + + sensor_msgs::Temperature temp; + temp.header.frame_id = "imu_frame"; + temp.temperature = 20.0 + ix; + // The ros::Time flavour of WriteMessage() + EXPECT_TRUE(bag.WriteMessage(temp, "/imu/temp", ros::Time(t0.sec_ + ix, t0.nsec_))); + } + bag.Close(); + } + + BagContents bag; + ASSERT_EQ(ReadBag(path_, bag), ""); + EXPECT_EQ(bag.conn_count_, 2u); + EXPECT_EQ(bag.chunk_count_, 1u); // Way below the chunk threshold + EXPECT_EQ(bag.n_index_, 2u); // One index record per connection per chunk + EXPECT_EQ(bag.topics_.at(0), "/imu/data"); + EXPECT_EQ(bag.topics_.at(1), "/imu/temp"); + EXPECT_EQ(bag.conn_hdrs_.at(0).at("type"), "sensor_msgs/Imu"); + EXPECT_EQ(bag.conn_hdrs_.at(0).at("md5sum"), ros::message_traits::md5sum()); + EXPECT_EQ(bag.conn_hdrs_.at(0).at("message_definition"), ros::message_traits::definition()); + EXPECT_EQ(bag.conn_hdrs_.at(1).at("type"), "sensor_msgs/Temperature"); + ASSERT_EQ(bag.msgs_.at(0).size(), 10u); + ASSERT_EQ(bag.msgs_.at(1).size(), 10u); + + // The message payloads must deserialise to what we put in + for (int ix = 0; ix < 10; ix++) { + const auto& data = bag.msgs_.at(0)[ix]; + std::vector raw(data.cbegin(), data.cend()); + sensor_msgs::Imu imu; + DeserializeMessage(raw, imu); + EXPECT_EQ(imu.header.seq, (uint32_t)ix); + EXPECT_EQ(imu.header.frame_id, "imu_frame"); + EXPECT_EQ(imu.header.stamp.sec, t0.sec_ + ix); + EXPECT_EQ(imu.header.stamp.nsec, t0.nsec_); + EXPECT_EQ(imu.linear_acceleration.x, (double)ix); + // ...and the bag record time must be right, too + EXPECT_EQ(bag.times_.at(0)[ix].sec_, t0.sec_ + ix); + EXPECT_EQ(bag.times_.at(0)[ix].nsec_, t0.nsec_); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST_F(BagWriterTest, MultipleChunks) +{ + // Write enough data to trigger more than one chunk (the threshold is 768 KiB) + const int n_msgs = 20; + const std::size_t msg_size = 100 * 1024; + { + BagWriter bag; + ASSERT_TRUE(bag.Open(path_)); + for (int ix = 0; ix < n_msgs; ix++) { + std_msgs::ByteMultiArray msg; + msg.data.assign(msg_size, (int8_t)ix); + EXPECT_TRUE(bag.WriteMessage(msg, "/big/raw", RosTime(1750000000 + ix, 0))); + } + bag.Close(); + } + + BagContents bag; + ASSERT_EQ(ReadBag(path_, bag), ""); + EXPECT_EQ(bag.conn_count_, 1u); + EXPECT_GE(bag.chunk_count_, 2u); + EXPECT_EQ(bag.chunk_count_, bag.n_index_); // One connection -> one index record per chunk + ASSERT_EQ(bag.msgs_.at(0).size(), (std::size_t)n_msgs); + for (int ix = 0; ix < n_msgs; ix++) { + const auto& data = bag.msgs_.at(0)[ix]; + std::vector raw(data.cbegin(), data.cend()); + std_msgs::ByteMultiArray msg; + DeserializeMessage(raw, msg); + ASSERT_EQ(msg.data.size(), msg_size); + EXPECT_EQ(msg.data[0], (int8_t)ix); + EXPECT_EQ(msg.data[msg_size - 1], (int8_t)ix); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST_F(BagWriterTest, MessagesFromFpl) +{ + // Serialise a message the way it is stored in a .fpl file + sensor_msgs::Imu imu; + imu.header.frame_id = "fpl_frame"; + imu.angular_velocity.y = 3.25; + const uint32_t size = ros::serialization::serializationLength(imu); + std::vector data(size); + ros::serialization::OStream stream(data.data(), size); + ros::serialization::serialize(stream, imu); + + fpsdk::common::fpl::RosMsgDef def; + def.valid_ = true; + def.info_ = "test"; + def.topic_name_ = "/fpl/imu"; + def.msg_name_ = ros::message_traits::datatype(); + def.msg_md5_ = ros::message_traits::md5sum(); + def.msg_def_ = ros::message_traits::definition(); + + fpsdk::common::fpl::RosMsgBin bin; + bin.valid_ = true; + bin.topic_name_ = def.topic_name_; + bin.rec_time_ = RosTime(1750000123, 456); + bin.msg_data_ = data; + + { + BagWriter bag; + ASSERT_TRUE(bag.Open(path_)); + + // Without a message definition this must fail -- noisy + EXPECT_FALSE(bag.WriteMessage(bin)); + + bag.AddMsgDef(def); + EXPECT_TRUE(bag.WriteMessage(bin)); + EXPECT_TRUE(bag.WriteMessage(bin)); + + // An invalid message is silently ignored + fpsdk::common::fpl::RosMsgBin invalid; + EXPECT_FALSE(bag.WriteMessage(invalid)); + + bag.Close(); + } + + BagContents bag; + ASSERT_EQ(ReadBag(path_, bag), ""); + EXPECT_EQ(bag.conn_count_, 1u); + EXPECT_EQ(bag.topics_.at(0), "/fpl/imu"); + // The .fpl connection header is used as-is (and it also has the "topic" field) + EXPECT_EQ(bag.conn_hdrs_.at(0).at("type"), def.msg_name_); + EXPECT_EQ(bag.conn_hdrs_.at(0).at("md5sum"), def.msg_md5_); + EXPECT_EQ(bag.conn_hdrs_.at(0).at("message_definition"), def.msg_def_); + EXPECT_EQ(bag.conn_hdrs_.at(0).at("topic"), def.topic_name_); + ASSERT_EQ(bag.msgs_.at(0).size(), 2u); + // The serialised data is written verbatim + EXPECT_TRUE(bag.msgs_.at(0)[0] == std::string(data.cbegin(), data.cend())); + EXPECT_EQ(bag.times_.at(0)[1].sec_, 1750000123u); + EXPECT_EQ(bag.times_.at(0)[1].nsec_, 456u); +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST_F(BagWriterTest, EmptyBag) +{ + { + BagWriter bag; + ASSERT_TRUE(bag.Open(path_)); + bag.Close(); + } + BagContents bag; + ASSERT_EQ(ReadBag(path_, bag), ""); + EXPECT_EQ(bag.conn_count_, 0u); + EXPECT_EQ(bag.chunk_count_, 0u); +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST_F(BagWriterTest, ErrorCases) +{ + BagWriter bag; + + // Writing without a bag open fails, but doesn't blow up + sensor_msgs::Imu imu; + EXPECT_FALSE(bag.WriteMessage(imu, "/imu/data")); + // Closing without a bag open is fine + bag.Close(); + + // Bad path -- noisy + EXPECT_FALSE(bag.Open("/no/such/directory/nope.bag")); + EXPECT_FALSE(bag.WriteMessage(imu, "/imu/data")); + +#if FPSDK_USE_BZ2 + // Compression is not available + EXPECT_TRUE(bag.Open(path_, 2)); + EXPECT_TRUE(bag.WriteMessage(imu, "/imu/data", RosTime(1750000000, 0))); +#endif + + // Re-opening implicitly closes the previous bag + ASSERT_TRUE(bag.Open(path_)); + EXPECT_TRUE(bag.WriteMessage(imu, "/imu/data", RosTime(1750000000, 0))); + bag.Close(); + + BagContents contents; + ASSERT_EQ(ReadBag(path_, contents), ""); + EXPECT_EQ(contents.conn_count_, 1u); + EXPECT_EQ(contents.msgs_.at(0).size(), 1u); +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST_F(BagWriterTest, UnorderedTimes) +{ + // The index records must be sorted by time even if the messages aren't written in time order + { + BagWriter bag; + ASSERT_TRUE(bag.Open(path_)); + sensor_msgs::Temperature msg; + for (const uint32_t sec : { 1750000005u, 1750000001u, 1750000003u, 1750000002u }) { + EXPECT_TRUE(bag.WriteMessage(msg, "/imu/temp", RosTime(sec, 0))); + } + bag.Close(); + } + // ReadBag() checks that the index is sorted + BagContents bag; + ASSERT_EQ(ReadBag(path_, bag), ""); + ASSERT_EQ(bag.msgs_.at(0).size(), 4u); + // ...but the messages themselves stay in the order they were written + EXPECT_EQ(bag.times_.at(0)[0].sec_, 1750000005u); + EXPECT_EQ(bag.times_.at(0)[3].sec_, 1750000002u); +} + +// --------------------------------------------------------------------------------------------------------------------- + +#if FPSDK_USE_BZ2 +// Write the same data compressed and uncompressed, and compare +TEST_F(BagWriterTest, Bz2Compression) +{ + // Nicely compressible data + const int n_msgs = 30; + const std::size_t msg_size = 64 * 1024; + const auto write_bag = [&](const std::string& path, const int compress) { + BagWriter bag; + EXPECT_TRUE(bag.Open(path, compress)); + for (int ix = 0; ix < n_msgs; ix++) { + std_msgs::ByteMultiArray msg; + msg.layout.dim.resize(1); + msg.layout.dim[0].label = "test"; + msg.data.assign(msg_size, (int8_t)(ix % 7)); + EXPECT_TRUE(bag.WriteMessage(msg, "/big/raw", RosTime(1750000000 + ix, 0))); + } + bag.Close(); + }; + + write_bag(path_, 0); + BagContents plain; + ASSERT_EQ(ReadBag(path_, plain), ""); + EXPECT_EQ(plain.compression_, "none"); + EXPECT_EQ(plain.compressed_, plain.uncompressed_); + + const std::string comp_path = path_ + ".bz2.bag"; + write_bag(comp_path, 1); // Note: 1 means LZ4 in rosbag, but we don't have that, so it's bz2, too + BagContents comp; + const std::string res = ReadBag(comp_path, comp); + std::remove(comp_path.c_str()); + ASSERT_EQ(res, ""); + +# if FPSDK_USE_BZ2 + EXPECT_EQ(comp.compression_, "bz2"); + EXPECT_LT(comp.compressed_, comp.uncompressed_ / 10); // This data compresses very well +# else + EXPECT_EQ(comp.compression_, "none"); // No bz2 available, it only warns + EXPECT_EQ(comp.compressed_, comp.uncompressed_); +# endif + + // Same content either way + EXPECT_EQ(comp.uncompressed_, plain.uncompressed_); + EXPECT_EQ(comp.chunk_count_, plain.chunk_count_); + EXPECT_EQ(comp.conn_count_, plain.conn_count_); + EXPECT_EQ(comp.conn_hdrs_, plain.conn_hdrs_); + ASSERT_EQ(comp.msgs_.at(0).size(), (std::size_t)n_msgs); + EXPECT_TRUE(comp.msgs_ == plain.msgs_); + for (int ix = 0; ix < n_msgs; ix++) { + const auto& data = comp.msgs_.at(0)[ix]; + std::vector raw(data.cbegin(), data.cend()); + std_msgs::ByteMultiArray msg; + DeserializeMessage(raw, msg); + ASSERT_EQ(msg.data.size(), msg_size); + EXPECT_EQ(msg.data[0], (int8_t)(ix % 7)); + ASSERT_EQ(msg.layout.dim.size(), 1u); + EXPECT_EQ(msg.layout.dim[0].label, "test"); + } +} +#endif + +// --------------------------------------------------------------------------------------------------------------------- + +#if FPSDK_USE_ROS1 +// This should be the last test as it messes with the console and we may want to use DEBUG() etc. in the tests +TEST(UtilsTest, RedirectLoggingToRosConsole) +{ + // Silence the ROS console + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); + + // We're not actually testing much more than checking that this doesn't crash. + // Run the test with -v -v -v and set ROS console level to Debug (above) to make it a bit more interesting in the + // output. + // clang-format off + // $ clear; make INSTALL_PREFIX=fpsdk BUILD_TYPE=Debug build && ROSCONSOLE_FORMAT='${severity} ${time:%Y-%m-%d %H:%M:%S.%f} ${logger} - ${message}' build/Debug/fpsdk_common/fpsdk_common_ros1_test -v -v -v + // clang-format on + + ROS_DEBUG("Hello, this is a ros debug before redirect..."); + ROS_INFO("Hello, this is a ros info before redirect..."); + // ROS_WARN("Hello, this is a ros warn before redirect..."); + DEBUG("This is fpsdk_common debug before redirect..."); + INFO("This is fpsdk_common info before redirect..."); + // WARNING("This is fpsdk_common warning before redirect..."); + + RedirectLoggingToRosConsole(); + + ROS_DEBUG("Hello, this is a ros debug after redirect..."); + ROS_INFO("Hello, this is a ros info after redirect..."); + // ROS_WARN("Hello, this is a ros warn after redirect..."); + DEBUG("This is fpsdk_common debug after redirect..."); + INFO("This is fpsdk_common info after redirect..."); + // WARNING("This is fpsdk_common warning after redirect..."); } +// This should be the last test as it messes with the console and we may want to use DEBUG() etc. in the tests +#endif /* ****************************************************************************************************************** */ } // namespace @@ -41,6 +772,9 @@ int main(int argc, char** argv) level++; } } +#if FPSDK_USE_ROS1 + ros::Time::init(); +#endif fpsdk::common::logging::LoggingSetParams(level); return RUN_ALL_TESTS(); } diff --git a/fpsdk_ros2/test/utils_test.cpp b/fpsdk_common/test/ros2_test.cpp similarity index 79% rename from fpsdk_ros2/test/utils_test.cpp rename to fpsdk_common/test/ros2_test.cpp index 1eff61f4..9a6d8e55 100644 --- a/fpsdk_ros2/test/utils_test.cpp +++ b/fpsdk_common/test/ros2_test.cpp @@ -8,23 +8,31 @@ * \endverbatim * * @file - * @brief Fixposition SDK: tests for fpsdk::ros2::utils + * @brief Fixposition SDK: tests for fpsdk::common::ros2 */ /* LIBC/STL */ /* EXTERNAL */ #include +#if FPSDK_USE_BZ2 +# include +#endif /* PACKAGE */ #include -#include +#include namespace { /* ****************************************************************************************************************** */ -using namespace fpsdk::ros2::utils; +#if FPSDK_USE_ROS2 -TEST(UtilsTest, ConvTime) +using namespace fpsdk::common::ros2; +using namespace fpsdk::common::time; + +// --------------------------------------------------------------------------------------------------------------------- + +TEST(Ros2Test, ConvTime) { const rclcpp::Time ros_time(1723650283, 125000000); DEBUG("rclcpp::Time %" PRIu64, ros_time.nanoseconds()); @@ -39,6 +47,7 @@ TEST(UtilsTest, ConvTime) EXPECT_EQ(ros_time.nanoseconds(), ros_time_again.nanoseconds()); } +#endif /* ****************************************************************************************************************** */ } // namespace diff --git a/fpsdk_doc/Doxyfile b/fpsdk_doc/Doxyfile index 04cccc65..a943ccde 100644 --- a/fpsdk_doc/Doxyfile +++ b/fpsdk_doc/Doxyfile @@ -986,11 +986,7 @@ WARN_LOGFILE = INPUT = fpsdk_doc \ fpsdk_common/doc \ - fpsdk_ros1/doc \ - fpsdk_ros2/doc \ fpsdk_common/include \ - fpsdk_ros1/include \ - fpsdk_ros2/include \ fpsdk_apps/doc \ fpsdk_apps/fpltool/fpltool_doc.hpp \ fpsdk_apps/parsertool/parsertool_doc.hpp \ @@ -1085,10 +1081,6 @@ EXCLUDE_SYMBOLS = EXAMPLE_PATH = fpsdk_doc \ fpsdk_common \ fpsdk_common/doc \ - fpsdk_ros1 \ - fpsdk_ros1/doc \ - fpsdk_ros2 \ - fpsdk_ros2/doc \ fpsdk_apps \ fpsdk_apps/doc \ examples/parser_intro \ @@ -1115,9 +1107,7 @@ EXAMPLE_RECURSIVE = YES # \image command). IMAGE_PATH = fpsdk_doc \ - fpsdk_common/doc \ - fpsdk_ros1/doc \ - fpsdk_apps/doc + fpsdk_common/doc # The INPUT_FILTER tag can be used to specify a program that Doxygen should # invoke to filter for each input file. Doxygen will invoke the filter program diff --git a/fpsdk_doc/fpsdk-overview.drawio.svg b/fpsdk_doc/fpsdk-overview.drawio.svg index dacf2c25..3e05fde7 100644 --- a/fpsdk_doc/fpsdk-overview.drawio.svg +++ b/fpsdk_doc/fpsdk-overview.drawio.svg @@ -1,14 +1,14 @@ - + - + -
+
System dependencies @@ -16,14 +16,14 @@
- + System dependencies - + @@ -93,14 +93,14 @@ - - + + -
+

@@ -114,21 +114,21 @@

- + yaml-cpp - - + + -
+

@@ -142,21 +142,21 @@

- + roscpp etc. - - + + -
+

@@ -170,21 +170,21 @@

- + boost - - + + -
+

@@ -198,21 +198,21 @@

- + zlib1g - - + + -
+

@@ -226,7 +226,7 @@

- + GCC @@ -259,13 +259,13 @@ - + -
+
parser @@ -273,36 +273,15 @@
- + parser - - - - - - - - - - -
-
-
- coming soon -
-
-
- - - coming soon - - - + + @@ -330,14 +309,14 @@ - - + + -
+

@@ -351,21 +330,21 @@

- + Linux - - + + -
+

@@ -379,21 +358,21 @@

- + glibc - - + + -
+

@@ -407,21 +386,21 @@

- + curl - - + + -
+

@@ -435,21 +414,21 @@

- + Eigen - - + + -
+

@@ -463,21 +442,21 @@

- + GTest - - + + -
+

@@ -489,21 +468,21 @@

- + PROJ - - + + -
+

@@ -517,20 +496,20 @@

- + rclcpp etc. - + -
+
optional @@ -538,20 +517,20 @@
- + optional - + -
+
optional @@ -559,7 +538,7 @@
- + optional @@ -611,7 +590,7 @@ - +
@@ -629,15 +608,15 @@ - - + + -
-
+
+

@@ -650,49 +629,21 @@

- + fpsdk::common - - - - - - - - -
-
-
-

- - - fpsdk::ros1 - - -

-
-
-
- - - fpsdk::ros1 - - - - - + -
+
utils @@ -700,25 +651,25 @@
- + utils - - + + - + -
+
path @@ -726,25 +677,25 @@
- + path - - + + - + -
+
math @@ -752,25 +703,25 @@
- + math - - + + - + -
+
app @@ -778,25 +729,25 @@
- + app - - + + - + -
+
fpl @@ -804,25 +755,25 @@
- + fpl - - + + - + -
+
logging @@ -830,25 +781,25 @@
- + logging - - + + - + -
+
string @@ -856,51 +807,25 @@
- + string - - + + - + -
-
-
- bagwriter -
-
-
- - - bagwriter - - - - - - - - - - - - - - - - -
+
trafo @@ -908,25 +833,25 @@
- + trafo - - + + - + -
+
video @@ -934,105 +859,25 @@
- + video - - - - - - - - - - - -
-
-
- utils -
-
-
- - - utils - - - - - - - - - - - - - - - - -
-
-
-

- - - fpsdk::ros2 - - -

-
-
-
- - - fpsdk::ros2 - - - + + - + -
-
-
- utils -
-
-
- - - utils - - - - - - - - - - - - - - - - -
+
parser @@ -1040,24 +885,24 @@
- + parser - - + + - + -
+
optional @@ -1065,65 +910,21 @@
- + optional - - - - - - - -
-
-
- Noetic -
-
-
- - - Noetic - - - - - - - - - - - -
-
-
- Humble, -
- Jazzy, Lyrical -
-
-
- - - Humble,... - - - - - + -
+
thread @@ -1131,24 +932,24 @@
- + thread - - + + - + -
+
c++17 @@ -1156,7 +957,7 @@
- + c++17 @@ -1229,14 +1030,14 @@ - - + + -
+

@@ -1248,21 +1049,21 @@

- + OpenSSL - - + + -
+

@@ -1274,7 +1075,7 @@

- + nloh...-json3 @@ -1331,13 +1132,13 @@ - + -
+
optional @@ -1345,7 +1146,7 @@
- + optional @@ -1378,309 +1179,367 @@ - + + + + -
+
- ros1 + findsensor
- - ros1 + + findsensor - - - - - + -
+
- bagwriter + yaml
- - bagwriter + + yaml - - + + - - + + - + + + +
+
+
+

+ + FFmpeg (LGPL) + +

+
+
+
+ + + FFmpeg (LGPL) + + + + + + -
-
+
+
- findsensor + optional
- - findsensor + + optional - + -
+
- yaml + cam
- - yaml + + cam - - + + + - - + -
-
-
-

- - FFmpeg (LGPL) - -

+
+
+
+ can
- - FFmpeg (LGPL) + + can - + + + + + + -
-
+
+
- optional + gnss
- - optional + + gnss + + + + - + -
+
- cam + ros2
- - cam + + ros2 - - + + - + -
+
- can + time
- - can + + time - - + + - + -
+
- gnss + types
- - gnss + + types - - + + - - + -
-
+
+
- ros1 + *
- - ros1 + + * - - + + + + + + +
+
+
+ ** +
+
+
+ + + ** + + + - - + -
+
-
- time +
+ * ROS1 Noetic
- - time + + * ROS1 Noetic - - + + + + + + +
+
+
+ ** ROS2 Humble, Jazzy or Lyrical +
+
+
+ + + ** ROS2 Humble, Jazzy or Lyrical + + + - + -
+
- types + ros1
- - types + + ros1 - - + + diff --git a/fpsdk_doc/fpsdk_build.hpp b/fpsdk_doc/fpsdk_build.hpp index c12e8b4d..40cc9df2 100644 --- a/fpsdk_doc/fpsdk_build.hpp +++ b/fpsdk_doc/fpsdk_build.hpp @@ -26,7 +26,7 @@ namespace fpsdk { This page describes how the Fixposition SDK can be built and installed. This is intended for users who are familiar with developing c++ code and building software and who want to build their own applications using the @ref - FPSDK_COMMON_DOC (etc.) + FPSDK_COMMON_DOC. For users interested in simply *using* the @ref FPSDK_APPS_DOC it is recommended to use the @ref FPSDK_RUN_DOC instead of building and installing those themselves. @@ -34,32 +34,44 @@ namespace fpsdk { @section FPSDK_BUILD_DEPS Dependencies - For building the libraries and apps: - - - **Linux**, **GCC** (C++-17), glibc, cmake, bash, etc. (tested with Ubuntu 20.04/22.04/24.04 and Debian Trixie) - - boost (≥ 1.71.0) - - curl (≥ 7.68.0) - - Eigen3 (≥ 3.3.7) - - yaml-cpp (≥ 0.6.2) - - zlib1g (≥ 1.2.11) - - OpenSSL (libssl) (≥ 1.1.x) - - nlohmann-json3 (≥ 3.7.3) - - Various Linux tools, including Bash, CMake, make, xxd, sed, awk, ... - - PROJ (*) (≥ 9.4.x) - - FFmpeg (libavcodec, libavutil, ...) (**) (= 7.1.x) - - ROS1 (*) (Noetic), or - - ROS2 (*) (Humble, Jazzy or Lyrical) - - (*) Optional dependencies. Without these some functionality in the libraries and apps is unavailable (compiled out). - - (**) Optional. The FFmpeg libraries must be configured with --disable-gpl and --disable-nonfree in order to comply - with the Fixposition SDK license. See *Configure* below for related build configuration options. - - For development additionally: - - - clang-format (≥ 19, tested with 19) - - Doxygen (≥ 1.11.0, tested with 1.14.0) - - GTest (≥ 1.13.0) + The Fixposition SDK is made for **Linux**. To build it a **GCC** (C++-17) toolchain is required. You may have some + luck using clang, but you're on your own there. Besides a glibc and standard Linux tools, such as CMake, make, bash, + xxd, sed, awk, etc. are required to build. This is tested and known to work with Ubuntu 20.04, 22.04, 24.04/ and + 26.04, as well as Debian Trixie. + + For building the libraries and apps the following dependencies are used. Some are required and some are optional. + + | Dependency | Version | Required | CMake (1) | Optional use | + |--------------------|-----------|----------|---------------------|--------------------------| + | boost | ≥ 1.71.0 | required | | | + | BZip2 | ≥ 1.0.8 | optional | FPSDK_USE_BZ2 | fpsdk::common::ros1::BagWriter | + | curl | ≥ 7.68.0 | required | | | + | Eigen3 | ≥ 3.3.7 | required | | | + | yaml-cpp | ≥ 0.6.2 | required | | | + | zlib1g | ≥ 1.2.11 | required | | | + | OpenSSL (libssl) | ≥ 1.1.x | required | | | + | nlohmann-json3 | ≥ 3.7.3 | required | | | + | PROJ | ≥ 9.4.x | optional | FPSDK_USE_PROJ | fpsdk::common::trafo::Transformer | + | FFmpeg (2) | = 7.1.x | optional | FPSDK_USE_FFMPEG | fpsdk::common::video, @ref FPSDK_APPS_FPLTOOL | + | ROS1 (3) | Noetic | optional | | fpsdk::common::ros1 | + | ROS2 (3) | Humble, Jazzy or Lyrical | optional| | fpsdk::common::ros2, @ref FPSDK_APPS_FPLTOOL | + | clang-format (4) | ≥ 19 | optional | | for development | + | Doxygen (4) | ≥ 1.14.0 | optional | | for development | + | GTest (4) | ≥ 1.13.0 | optional | FPSDK_BUILD_TESTING | for development | + + (1) These CMake arguments are available to control these dependencies. By default the dependency is automatically + used when a suitable library is found. To explicitly enable or disable the use of such a dependency one can set + the CMake argument accordingly. For example: `-DFPSDK_USE_PROJ=OFF` to disable the use of the PROJ library or + `-DFPSDK_USE_PROJ=ON` to required the use of the PROJ library. See also @ref FPSDK_BUILD_BUILD_MANUAL below. + + (2) The FFmpeg libraries must be configured with --disable-gpl and --disable-nonfree in order to comply + with the Fixposition SDK license. + + (3) ROS support is optional, and it's either ROS1 *or* ROS2. To enable, build in a ROS environment using catkin + resp. colcon and the availability of ROS is detected automatically. + + (4) These are only needed for development. That is, they are not required for building the \@ref FPSDK_COMMON_DOC + and @ref FPSDK_APPS_DOC. See @ref FPSDK_BUILD_CIVERSIONS for the versions used in the CI builds. @@ -72,10 +84,9 @@ namespace fpsdk { @subsection FPSDK_BUILD_BUILD_TLDR tl;dr @code{sh} - ./docker/docker.sh pull noetic-dev # Or "docker.sh build noetic-dev" to build the image locally - ./docker/docker.sh run noetic-dev bash + ./docker/docker.sh pull trixie-dev # Or "docker.sh build trixie-dev" to build the image locally + ./docker/docker.sh run trixie-dev bash # Now inside Docker do: - source /opt/ros/noetic/setup.bash make install ./fpsdk/bin/fpltool -h @endcode @@ -84,7 +95,8 @@ namespace fpsdk { @subsection FPSDK_BUILD_BUILD_DEVCONTAINER VSCode devcontainer - Open the fpsdk.code-workspace, change to one of the provided devcontainers, and in a terminal do: + Open the fpsdk.code-workspace, change to one of the provided devcontainers (recommended: trixie), and in a terminal + do: @code{sh} make install @@ -97,24 +109,29 @@ namespace fpsdk { Docker images are provided that include all the dependencies: + @code{sh} + ./docker/docker.sh pull trixie-dev # Or "docker.sh build trixie-dev" to build the image locally + ./docker/docker.sh run trixie-dev bash + make install + ./fpsdk/bin/fpltool + @endcode + + Note that for the containers with ROS (Noetic, Lyrical, etc.) you'll have to source the ROS stuff. For example: + @code{sh} ./docker/docker.sh pull noetic-dev # Or "docker.sh build noetic-dev" to build the image locally ./docker/docker.sh run noetic-dev bash - source /opt/ros/noetic/setup.bash + . /opt/ros/noetic/setup.bash make install ./fpsdk/bin/fpltool @endcode - Note that the "docker.sh" script does not give you a suitable ROS runtime (or development, playground, ...) - environment! Its only purpose is to run the CI for this repo and to demonstrate the building here. For a ROS - run-time environment please setup a docker *container* yourself elsewhere (perhaps using the docker *image* provided - here). See ROS and Docker documentation and the Internet for help. - @subsection FPSDK_BUILD_BUILD_CI Run CI @code{sh} + ./docker/docker.sh run trixie-ci ./docker/ci.sh ./docker/docker.sh run noetic-ci ./docker/ci.sh ./docker/docker.sh run humble-ci ./docker/ci.sh ./docker/docker.sh run jazzy-ci ./docker/ci.sh @@ -133,12 +150,15 @@ namespace fpsdk { 1. Setup build system, install dependencies The exact steps required depend on your system. You'll need the dependencies mentioned above installed system - wide or otherwise tell CMake where to find them. + wide or otherwise tell CMake where to find them. Refer to the provided Docker configuration and helper scripts, + namely "install_apt_base.sh", to check which packages can be installed in Ubuntu or Debian. + + Something like this should work: @code{sh} sudo apt install libyaml-cpp-dev libboost-all-dev zlib1g-dev libeigen3-dev linux-libc-dev xxd # For building sudo apt install libgtest-dev clang-format doxygen pre-commit # For development - source /opt/ros/noetic/setup.bash # If you have ROS1 + source /opt/ros/lyrical/setup.bash # If you have ROS2 Lyrical @endcode 3. Configure @@ -147,17 +167,10 @@ namespace fpsdk { cmake -B build -DCMAKE_INSTALL_PREFIX=~/fpsdk @endcode - Additional parameters include (see CMakeList.txt files of the projects for details and more parameters): + Additional parameters, such as `-DFPSDK_USE_PROJ=OFF`, can be given. See @ref FPSDK_BUILD_DEPS above. - - Build type: `-DCMAKE_BUILD_TYPE=Debug` or `-DCMAKE_BUILD_TYPE=Release` (default) - - Force ROS1 package path: `-DROS_PACKAGE_PATH=/path/to/ros` (default: auto-detect) - - Explicitly enable or disable testing: `-DFPSDK_BUILD_TESTING=ON` or `-DFPSDK_BUILD_TESTING=OFF`. - The default is to automatically enable testing if a suitable GTest library is found. - - Explicitly enable or disable use of the PROJ library: `-DFPSDK_USE_PROJ=ON` or `-DFPSDK_USE_PROJ=OFF`. - The default is to automatically use the PROJ library if a suitable version is found. - - Explicitly enable or disable use of the FFmpeg libraries: `-DFPSDK_USE_FFMPEG=ON` or `-DFPSDK_USE_FFMPEG=OFF`. - The default is to automatically use the FFmpeg libraries if suitable versions are found. - - Add `-DCMAKE_PREFIX_PATH=...` to hint at non-standard installation paths, such as `/path/to/ffmpeg-lgpl`. + The build type can be selected by `-DCMAKE_BUILD_TYPE=Debug` or `-DCMAKE_BUILD_TYPE=Release` (default). + Add `-DCMAKE_PREFIX_PATH=...` to hint at non-standard installation paths, such as `/path/to/ffmpeg-lgpl`. 4. Build @@ -211,21 +224,19 @@ namespace fpsdk { catkin build fpsdk_common @endcode - Note that if you clone this repository directly to your `ros_workspace/src` directory, you'll have to place + Note that if you clone this repository directly to your `ros_workspace/src` directory, you may have to place CATKIN_IGNORE resp. COLCON_IGNORE files in some places. For example: @code{sh} # ROS1 catkin workspace touch src/fixposition-sdk/examples/CATKIN_IGNORE # Ignore all examples, or - touch src/fixposition-sdk/examples/ros1_fpsdk_demo/CATKIN_IGNORE # Ignore only this example - touch src/fixposition-sdk/fpsdk_ros2/CATKIN_IGNORE + touch src/fixposition-sdk/examples/parser_intro/CATKIN_IGNORE # Ignore only this example @endcode @code{sh} # ROS2 colcon workspace touch src/fixposition-sdk/examples/COLCON_IGNORE # Ignore all examples, or - touch src/fixposition-sdk/examples/ros2_fpsdk_demo/COLCON_IGNORE # Ignore only this example - touch src/fixposition-sdk/fpsdk_ros1/COLCON_IGNORE + touch src/fixposition-sdk/examples/parser_intro/COLCON_IGNORE # Ignore only this example @endcode diff --git a/fpsdk_doc/fpsdk_doc.hpp b/fpsdk_doc/fpsdk_doc.hpp index 6c9930a7..c8e289bc 100644 --- a/fpsdk_doc/fpsdk_doc.hpp +++ b/fpsdk_doc/fpsdk_doc.hpp @@ -40,8 +40,6 @@ namespace fpsdk { The SDK consists of the following packages: - @subpage FPSDK_COMMON_DOC - - @subpage FPSDK_ROS1_DOC - - @subpage FPSDK_ROS2_DOC - @subpage FPSDK_APPS_DOC @section FPSDK_EXAMPLES Examples @@ -60,10 +58,10 @@ namespace fpsdk { - Fixposition SDK source code repository: https://github.com/fixposition/fixposition-sdk - @section FPSDK_LICENSE Licenses + @section FPSDK_LICENSE License See the LICENSE file included in the source of each package or the generated documentation (@ref - FPSDK_COMMON_LICENSE, @ref FPSDK_ROS1_LICENSE, @ref FPSDK_ROS2_LICENSE and @ref FPSDK_APPS_LICENSE). + FPSDK_COMMON_LICENSE and @ref FPSDK_APPS_LICENSE). */ // clang-format on diff --git a/fpsdk_doc/fpsdk_examples.hpp b/fpsdk_doc/fpsdk_examples.hpp index 1067763e..4bb48a65 100644 --- a/fpsdk_doc/fpsdk_examples.hpp +++ b/fpsdk_doc/fpsdk_examples.hpp @@ -25,8 +25,6 @@ namespace fpsdk { - @subpage FPSDK_EXAMPLES_FUSION_EPOCH - @subpage FPSDK_EXAMPLES_FPB_MEASUREMENTS - @subpage FPSDK_EXAMPLES_CAMERA_STREAMING - - @subpage FPSDK_EXAMPLES_ROS1_DEMO - - @subpage FPSDK_EXAMPLES_ROS2_DEMO @page FPSDK_EXAMPLES_PARSER_INTRO Parsing and decoding FP_A messages @@ -48,17 +46,6 @@ namespace fpsdk { @include{lineno} camera_streaming.cpp - - @page FPSDK_EXAMPLES_ROS1_DEMO ROS 1 node - - This is a demonstratation of using the Fixposition SDK in a simple ROS 1 node. See the examples/ros1_fpsdk_demo - directory in the source code repository for details. - - - @page FPSDK_EXAMPLES_ROS2_DEMO ROS 2 node - - This is a demonstratation of using the Fixposition SDK in a simple ROS 2 node. See the examples/ros2_fpsdk_demo - directory in the source code repository for details. */ // clang-format on diff --git a/fpsdk_ros1/CMakeLists.txt b/fpsdk_ros1/CMakeLists.txt deleted file mode 100644 index 774b993a..00000000 --- a/fpsdk_ros1/CMakeLists.txt +++ /dev/null @@ -1,165 +0,0 @@ -# GENERAL ============================================================================================================== -message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----") - -cmake_minimum_required(VERSION 3.16) -include(../fpsdk_common/cmake/setup.cmake) -include(../fpsdk_common/cmake/utils.cmake) - -project(fpsdk_ros1 - LANGUAGES CXX - VERSION ${FPSDK_VERSION_NUMBER} - DESCRIPTION "Fixposition SDK: ROS1 library" -) - - -# COMPILER SETUP ======================================================================================================= - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror \ - -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual") -set(CMAKE_CXX_FLAGS_RELEASE "-O3") -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() -if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") - add_compile_definitions(NDEBUG) -endif() - - -# DEPENDENCIES ========================================================================================================= - -# Handle this being used as add_subdirectory() of ../CMakeLists.txt vs. building this standalone -if(NOT TARGET fpsdk_common) - find_package(fpsdk_common REQUIRED) -endif() - -fpsdk_cmake_find_ros1_package(roscpp) -fpsdk_cmake_find_ros1_package(rosconsole) -fpsdk_cmake_find_ros1_package(rosbag) -fpsdk_cmake_find_ros1_package(std_msgs) -fpsdk_cmake_find_ros1_package(sensor_msgs) -fpsdk_cmake_find_ros1_package(geometry_msgs) -fpsdk_cmake_find_ros1_package(tf2_msgs) -fpsdk_cmake_find_ros1_package(nav_msgs) -fpsdk_cmake_find_ros1_package(topic_tools) - - -# SHARED LIBRARY ======================================================================================================= - -file(GLOB CPP_FILES src/*.cpp) -add_library(${PROJECT_NAME} SHARED ${CPP_FILES}) - -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ -) - -target_link_libraries(${PROJECT_NAME} - PUBLIC - fpsdk_common - PRIVATE - ros1::rosbag - ros1::roscpp - ros1::rosconsole - ros1::std_msgs - ros1::sensor_msgs - ros1::geometry_msgs - ros1::tf2_msgs - ros1::nav_msgs - ros1::topic_tools -) - -set_target_properties(${PROJECT_NAME} - PROPERTIES - VERSION ${PROJECT_VERSION} -) - - -# INSTALL ============================================================================================================== - -include(GNUInstallDirs) # Provides nice relative paths wrt CMAKE_INSTALL_PREFIX -set(PROJECT_RUNTIME_DIR ${CMAKE_INSTALL_FULL_BINDIR}) -set(PROJECT_LIBRARY_DIR ${CMAKE_INSTALL_FULL_LIBDIR}) -set(PROJECT_INCLUDE_DIR ${CMAKE_INSTALL_FULL_INCLUDEDIR}/${PROJECT_NAME}) -set(PROJECT_DATA_DIR ${CMAKE_INSTALL_FULL_DATAROOTDIR}/${PROJECT_NAME}) -set(PROJECT_DOC_DIR ${CMAKE_INSTALL_FULL_DOCDIR}) - -# Headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${PROJECT_INCLUDE_DIR} -) - -# Library, tools -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}-targets - LIBRARY DESTINATION ${PROJECT_LIBRARY_DIR} - RUNTIME DESTINATION ${PROJECT_RUNTIME_DIR} -) - -# CMake target config -install(EXPORT ${PROJECT_NAME}-targets - NAMESPACE ${PROJECT_NAMESPACE_PREFIX} - FILE ${PROJECT_NAME}-targets.cmake - DESTINATION lib/cmake/${PROJECT_NAME} -) - -# CMake config -include(CMakePackageConfigHelpers) -set(TARGET1 ${PROJECT_NAME}) -configure_package_config_file( - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/config.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake - INSTALL_DESTINATION lib/cmake/${PROJECT_NAME} - NO_SET_AND_CHECK_MACRO - NO_CHECK_REQUIRED_COMPONENTS_MACRO -) -write_basic_package_version_file( - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake - VERSION "${CMAKE_PROJECT_VERSION_MAJOR}.${CMAKE_PROJECT_VERSION_MINOR}" - COMPATIBILITY AnyNewerVersion -) -install( - FILES - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake - DESTINATION lib/cmake/${PROJECT_NAME} -) - -# pkg-config config -include(CMakePackageConfigHelpers) -set(TARGET1 ${PROJECT_NAME}) -configure_package_config_file( - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/config.pc.in - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc - INSTALL_DESTINATION lib/cmake/${PROJECT_NAME} - NO_SET_AND_CHECK_MACRO - NO_CHECK_REQUIRED_COMPONENTS_MACRO -) -install( - FILES - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc - DESTINATION lib/pkgconfig -) - -# Documentation -install( - FILES - ${PROJECT_SOURCE_DIR}/README.md - ${PROJECT_SOURCE_DIR}/LICENSE - DESTINATION ${PROJECT_DOC_DIR} -) - - -# TESTS ================================================================================================================ - -include(../fpsdk_common/cmake/testing.cmake) - -add_gtest(TARGET bagwriter_test SOURCES test/bagwriter_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole ros1::rosbag) -add_gtest(TARGET utils_test SOURCES test/utils_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole) - - -# ====================================================================================================================== diff --git a/fpsdk_ros1/LICENSE b/fpsdk_ros1/LICENSE deleted file mode 100644 index 52b9c765..00000000 --- a/fpsdk_ros1/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) Fixposition AG (www.fixposition.com) and 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. diff --git a/fpsdk_ros1/README.md b/fpsdk_ros1/README.md deleted file mode 100644 index cbb6f588..00000000 --- a/fpsdk_ros1/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# Fixposition SDK: ROS1 Library - -See the [../README.md](../README.md) for details. - -License: see the [LICENSE](LICENSE) file and [../README.md#license](../README.md#license). diff --git a/fpsdk_ros1/cmake/config.cmake.in b/fpsdk_ros1/cmake/config.cmake.in deleted file mode 100644 index 9463d461..00000000 --- a/fpsdk_ros1/cmake/config.cmake.in +++ /dev/null @@ -1,19 +0,0 @@ -@PACKAGE_INIT@ - -include ( "${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake" ) - -# library version information -set(@PROJECT_NAME@_VERSION_STRING "@PROJECT_VERSION@") -set(@PROJECT_NAME@_VERSION_MAJOR @PROJECT_VERSION_MAJOR@) -set(@PROJECT_NAME@_VERSION_MINOR @PROJECT_VERSION_MINOR@) -set(@PROJECT_NAME@_VERSION_PATCH @PROJECT_VERSION_PATCH@) - -set(@PROJECT_NAME@_FOUND 1) - -# Include and library dirs -set(_INSTALL_PREFIX "@CMAKE_INSTALL_PREFIX@") -set(@PROJECT_NAME@_INCLUDE_DIRS "${_INSTALL_PREFIX}/@CMAKE_INSTALL_INCLUDEDIR@") -set(@PROJECT_NAME@_LIBRARY_DIRS "${_INSTALL_PREFIX}/@CMAKE_INSTALL_LIBDIR@") - -# Libraries -set(@PROJECT_NAME@_LIBRARIES "@TARGET1@") diff --git a/fpsdk_ros1/cmake/config.pc.in b/fpsdk_ros1/cmake/config.pc.in deleted file mode 100644 index 3cc02c12..00000000 --- a/fpsdk_ros1/cmake/config.pc.in +++ /dev/null @@ -1,12 +0,0 @@ -prefix=@CMAKE_INSTALL_PREFIX@ -exec_prefix=@CMAKE_INSTALL_PREFIX@ -libdir=${exec_prefix}/@CMAKE_INSTALL_LIBDIR@ -includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ - -Name: @PROJECT_NAME@ -Description: @PROJECT_DESCRIPTION@ -Version: @PROJECT_VERSION@ - -Requires: -Libs: -L${libdir} -l@TARGET1@ -Cflags: -I${includedir} diff --git a/fpsdk_ros1/doc/doc.hpp b/fpsdk_ros1/doc/doc.hpp deleted file mode 100644 index 9c4e02eb..00000000 --- a/fpsdk_ros1/doc/doc.hpp +++ /dev/null @@ -1,47 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 library documentation - */ - -#ifndef __FPSDK_ROS1_DOC_HPP__ -#define __FPSDK_ROS1_DOC_HPP__ - -namespace fpsdk { -/** - * @brief Fixposition SDK: ROS1 library - */ -namespace ros1 { -// clang-format off -/* ****************************************************************************************************************** */ - -/*! - @page FPSDK_ROS1_DOC Fixposition SDK: ROS1 Library - - @section FPSDK_ROS1_OVERVIEW Overview - - @todo add documentation - - @section FPSDK_ROS1_MODULES Modules - - - @subpage FPSDK_ROS1_BAGWRITER - - @subpage FPSDK_ROS1_UTILS - - @section FPSDK_ROS1_LICENSE License - - @include fpsdk_ros1/LICENSE - -*/ - -/* ****************************************************************************************************************** */ -// clang-format on -} // namespace ros1 -} // namespace fpsdk -#endif // __FPSDK_ROS1_DOC_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp b/fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp deleted file mode 100644 index 068992cd..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp +++ /dev/null @@ -1,139 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 bag writer - * - * @page FPSDK_ROS1_BAGWRITER ROS1 bag writer - * - * **API**: fpsdk_ros1/bagwriter.hpp and fpsdk::ros1::bagwriter - * - */ -#ifndef __FPSDK_ROS1_BAGWRITER_HPP__ -#define __FPSDK_ROS1_BAGWRITER_HPP__ - -/* LIBC/STL */ -#include -#include -#include -#include - -/* EXTERNAL */ -#include "fpsdk_ros1/ext/ros_time.hpp" -#include "fpsdk_ros1/ext/rosbag_bag.hpp" - -/* Fixposition SDK */ -#include -#include - -/* PACKAGE */ - -namespace fpsdk { -namespace ros1 { -/** - * @brief ROS1 bag writer - */ -namespace bagwriter { -/* ****************************************************************************************************************** */ - -/** - * @brief ROS1 bag writer helper - */ -class BagWriter -{ - public: - BagWriter(); - ~BagWriter(); - - /** - * @brief Open bag for writing - * - * @param[in] path Path/filename of the bag file - * @param[in] compress Compress bag, 0 = no compression, 1 = LZ4, 2+ = BZ2 - * - * @returns true if bag was sucessfully opened - */ - bool Open(const std::string& path, const int compress = 0); - - /** - * @brief Close bag - */ - void Close(); - - /** - * @brief Write a message to the bag - * - * @tparam T ROS message type - * @param[in] msg The message - * @param[in] topic Topic name - * @param[in] time Bag record time - * - * @returns true if message was added, false otherwise (message definition missing) - */ - template - bool WriteMessage(const T& msg, const std::string& topic, const ros::Time& time) - { - bool ok = false; - if (bag_) { - try { - bag_->write(topic, time, msg); - ok = true; - } catch (const rosbag::BagException& ex) { - WARNING("BagWriter: write fail: %s", ex.what()); - } - } - return ok; - } - - /** - * @brief Write a message to the bag - * - * @tparam T ROS message type - * @param[in] msg The message - * @param[in] topic Topic name - * @param[in] time Bag record time - * - * @returns true if message was added, false otherwise (message definition missing) - */ - template - bool WriteMessage(const T& msg, const std::string& topic, const common::time::RosTime& time = {}) - { - return WriteMessage(msg, topic, ros::Time(time.sec_, time.nsec_)); - } - - /** - * @brief Add ROS message definition from .fpl - * - * @note No checks on the provided data are done! - * - * @param[in] rosmsgdef The message definition - */ - void AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef); - - /** - * @brief Write message from .fpl - * - * @note No checks on the provided data are done! - * - * @param[in] rosmsgbin The recorded message - * - * @returns true if message was added, false otherwise (message definition missing) - */ - bool WriteMessage(const common::fpl::RosMsgBin& rosmsgbin); - - private: - std::unique_ptr bag_; //!< Bag file handle - std::map> msg_defs_; //!< Message definitions (connection headers) -}; - -/* ****************************************************************************************************************** */ -} // namespace bagwriter -} // namespace ros1 -} // namespace fpsdk -#endif // __FPSDK_ROS1_BAGWRITER_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/cv_bridge.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/cv_bridge.hpp deleted file mode 100644 index 33df7e99..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/cv_bridge.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_CV_BRIDGE_HPP__ -#define __FPSDK_ROS1_EXT_CV_BRIDGE_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_CV_BRIDGE_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/eigen_conversions.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/eigen_conversions.hpp deleted file mode 100644 index 7b1340ef..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/eigen_conversions.hpp +++ /dev/null @@ -1,20 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_EIGEN_CONVERSIONS_HPP__ -#define __FPSDK_ROS1_EXT_EIGEN_CONVERSIONS_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wall" -#pragma GCC diagnostic ignored "-Wextra" -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" -#if defined(__GNUC__) && (__GNUC__ >= 9) -# pragma GCC diagnostic ignored "-Wdeprecated-copy" -#endif -#include -#pragma GCC diagnostic pop -// See commentes in fp_common/ext/eigen_core.hpp -#if !EIGEN_VERSION_AT_LEAST(3, 4, 0) && defined(__GNUC__) && (__GNUC__ >= 9) -# pragma GCC diagnostic ignored "-Wdeprecated-copy" -#endif -#endif // __FPSDK_ROS1_EXT_EIGEN_CONVERSIONS_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros.hpp deleted file mode 100644 index a434037a..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_HPP__ -#define __FPSDK_ROS1_EXT_ROS_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_callback_queue.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_callback_queue.hpp deleted file mode 100644 index fcdda4ed..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_callback_queue.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_CALLBACK_QUEUE_HPP__ -#define __FPSDK_ROS1_EXT_ROS_CALLBACK_QUEUE_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_CALLBACK_QUEUE_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_console.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_console.hpp deleted file mode 100644 index aabdc3c9..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_console.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_CONSOLE_HPP__ -#define __FPSDK_ROS1_EXT_ROS_CONSOLE_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_CONSOLE_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp deleted file mode 100644 index 42105f0f..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp +++ /dev/null @@ -1,11 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_MSGS_HPP__ -#define __FPSDK_ROS1_EXT_ROS_MSGS_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_MSGS_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_param.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_param.hpp deleted file mode 100644 index db0689f2..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_param.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_PARAMS_HPP__ -#define __FPSDK_ROS1_EXT_ROS_PARAMS_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_PARAMS_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_serialization.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_serialization.hpp deleted file mode 100644 index 54d1388c..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_serialization.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_SERIALIZATION_HPP__ -#define __FPSDK_ROS1_EXT_ROS_SERIALIZATION_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_SERIALIZATION_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_time.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_time.hpp deleted file mode 100644 index a4eeef5b..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_time.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_TIME_HPP__ -#define __FPSDK_ROS1_EXT_ROS_TIME_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_TIME_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_timer.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_timer.hpp deleted file mode 100644 index 6461b2a1..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_timer.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_TIMER_HPP__ -#define __FPSDK_ROS1_EXT_ROS_TIMER_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_TIMER_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_bag.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_bag.hpp deleted file mode 100644 index fef384ea..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_bag.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROS_ROSBAG_HPP__ -#define __FPSDK_ROS1_EXT_ROS_ROSBAG_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROS_ROSBAG_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_view.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_view.hpp deleted file mode 100644 index 2f8c7d27..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/rosbag_view.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_ROSBAG_VIEW_HPP__ -#define __FPSDK_ROS1_EXT_ROSBAG_VIEW_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_ROSBAG_VIEW_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/topic_tools.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/topic_tools.hpp deleted file mode 100644 index 0158b2e9..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/topic_tools.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS1_EXT_TOPIC_TOOLS_SHAPE_SHIFTER_HPP__ -#define __FPSDK_ROS1_EXT_TOPIC_TOOLS_SHAPE_SHIFTER_HPP__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS1_EXT_TOPIC_TOOLS_SHAPE_SHIFTER_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/utils.hpp b/fpsdk_ros1/include/fpsdk_ros1/utils.hpp deleted file mode 100644 index 10656c43..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/utils.hpp +++ /dev/null @@ -1,145 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 utilities - * - * @page FPSDK_ROS1_UTILS ROS1 utilities - * - * **API**: fpsdk_ros1/utils.hpp and fpsdk::ros1::utils - * - */ -#ifndef __FPSDK_ROS1_UTILS_HPP__ -#define __FPSDK_ROS1_UTILS_HPP__ - -/* LIBC/STL */ -#include -#include -#include - -/* EXTERNAL */ -#include -#include - -/* Fixposition SDK */ -#include - -/* PACKAGE */ - -namespace fpsdk { -namespace ros1 { -/** - * @brief ROS1 utilities - */ -namespace utils { -/* ****************************************************************************************************************** */ - -/** - * @brief Redirect fpsdk:common::logging to ROS console - * - * This configures the fpsdk::common::logging facility to output via the ROS console. This does *not* configure the ROS - * console (logger level, logger name, etc.). - * - * The mapping of fpsdk::common::logging::LoggingLevel to ros::console::levels is as follows: - * - * - TRACE and DEBUG --> DEBUG - * - INFO and NOTICE --> INFO - * - WARNING --> WARN - * - ERROR --> ERROR - * - FATAL --> FATAL - * - * @param[in] logger_name The name of the logger. The default value should give the caller package's - * ROSCONSOLE_DEFAULT_NAME, for example, "ros1_fpsdk_demo". That is, typically this argument - * should be left empty (the default value). - */ -void RedirectLoggingToRosConsole(const char* logger_name = ROSCONSOLE_DEFAULT_NAME /* = caller's package name */); - -/** - * @brief Loads a parameter from the ROS parameter server (int) - * - * @param[in] name The parameter name - * @param[out] value The value - * - * @returns true if parameter found and loaded, false otherwise - */ -bool LoadRosParam(const std::string& name, int& value); - -/** - * @brief Loads a parameter from the ROS parameter server (string) - * - * @param[in] name The parameter name - * @param[out] value The value - * - * @returns true if parameter found and loaded, false otherwise - */ -bool LoadRosParam(const std::string& name, std::string& value); - -/** - * @brief Loads a parameter from the ROS parameter server (bool) - * - * @param[in] name The parameter name - * @param[out] value The value - * - * @returns true if parameter found and loaded, false otherwise - */ -bool LoadRosParam(const std::string& name, bool& value); - -/** - * @brief Loads a parameter from the ROS parameter server (float) - * - * @param[in] name The parameter name - * @param[out] value The value - * - * @returns true if parameter found and loaded, false otherwise - */ -bool LoadRosParam(const std::string& name, float& value); - -/** - * @brief Loads a parameter from the ROS parameter server (double) - * - * @param[in] name The parameter name - * @param[out] value The value - * - * @returns true if parameter found and loaded, false otherwise - */ -bool LoadRosParam(const std::string& name, double& value); - -/** - * @brief Loads a parameter from the ROS parameter server (list of strings) - * - * @param[in] name The parameter name - * @param[out] value The value - * - * @returns true if parameter found and loaded, false otherwise - */ -bool LoadRosParam(const std::string& name, std::vector& value); - -/** - * @brief Convert to ROS time (atomic -> POSIX) - * - * @param[in] time The Time object (atomic) - * - * @returns the ROS time object (POSIX) - */ -ros::Time ConvTime(const fpsdk::common::time::Time& time); - -/** - * @brief Convert from ROS time (POSIX -> atomic) - * - * @param[in] time The ROS time object (POSIX) - * - * @returns the Time object (atomic) - */ -fpsdk::common::time::Time ConvTime(const ros::Time& time); - -/* ****************************************************************************************************************** */ -} // namespace utils -} // namespace ros1 -} // namespace fpsdk -#endif // __FPSDK_ROS1_UTILS_HPP__ diff --git a/fpsdk_ros1/package.xml b/fpsdk_ros1/package.xml deleted file mode 100644 index 2f13f0b5..00000000 --- a/fpsdk_ros1/package.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - fpsdk_ros1 - 1.0.0 - Fixposition SDK: ROS1 library - Fixposition Support - MIT - - cmake - - fpsdk_common - roscpp - rosbag - std_msgs - topic_tools - diff --git a/fpsdk_ros1/src/bagwriter.cpp b/fpsdk_ros1/src/bagwriter.cpp deleted file mode 100644 index 0c461d6a..00000000 --- a/fpsdk_ros1/src/bagwriter.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 bag writer - */ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include "fpsdk_ros1/ext/ros.hpp" -#include "fpsdk_ros1/ext/topic_tools.hpp" - -/* Fixposition SDK */ -#include - -/* PACKAGE */ -#include "fpsdk_ros1/bagwriter.hpp" - -namespace fpsdk { -namespace ros1 { -namespace bagwriter { -/* ****************************************************************************************************************** */ - -BagWriter::BagWriter() -{ -} - -BagWriter::~BagWriter() -{ - Close(); -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool BagWriter::Open(const std::string& path, const int compress) -{ - Close(); - bag_ = std::make_unique(); - const char* compress_str = "none"; - if (compress > 1) { - bag_->setCompression(rosbag::compression::CompressionType::BZ2); - compress_str = "bz2"; - } else if (compress > 0) { - bag_->setCompression(rosbag::compression::CompressionType::LZ4); - compress_str = "lz4"; - } - try { - bag_->open(path, rosbag::bagmode::Write); - } catch (const rosbag::BagException& ex) { - WARNING("BagWriter: open fail %s: %s", path.c_str(), ex.what()); - bag_.reset(); - return false; - } - DEBUG("BagWriter: %s (%s)", path.c_str(), compress_str); - return true; -} - -// --------------------------------------------------------------------------------------------------------------------- - -void BagWriter::Close() -{ - if (bag_) { - bag_->close(); - bag_.reset(); - } -} - -// --------------------------------------------------------------------------------------------------------------------- - -void BagWriter::AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef) -{ - if (rosmsgdef.valid_ && (msg_defs_.find(rosmsgdef.topic_name_) == msg_defs_.end())) { - DEBUG("BagWriter: %s", rosmsgdef.info_.c_str()); - auto hdr = boost::make_shared(); - hdr->emplace(std::string("message_definition"), rosmsgdef.msg_def_); - hdr->emplace(std::string("topic"), rosmsgdef.topic_name_); - hdr->emplace(std::string("md5sum"), rosmsgdef.msg_md5_); - hdr->emplace(std::string("type"), rosmsgdef.msg_name_); - msg_defs_.emplace(rosmsgdef.topic_name_, hdr); - } -} - -// --------------------------------------------------------------------------------------------------------------------- - -bool BagWriter::WriteMessage(const common::fpl::RosMsgBin& rosmsgbin) -{ - if (!rosmsgbin.valid_) { - return false; - } - // For ROS1 we can directly write the serialised data. We should already know the message meta data (see AddMsgDef() - // above). - - auto msg_defs_entry = msg_defs_.find(rosmsgbin.topic_name_); - if (msg_defs_entry == msg_defs_.end()) { - WARNING("BagWriter: missing message definition for %s", rosmsgbin.topic_name_.c_str()); - return false; - } - - struct ShapeShifterReadHelper - { /* clang-format off */ - ShapeShifterReadHelper(const uint8_t* data, const uint32_t size) : data_{data}, size_{size} {} - const uint8_t* data_; - const uint32_t size_; - std::size_t getLength() { return size_; } - const uint8_t* getData() { return data_; } - }; // clang-format on - - topic_tools::ShapeShifter ros_msg; - ShapeShifterReadHelper stream(rosmsgbin.msg_data_.data(), rosmsgbin.msg_data_.size()); - ros_msg.read(stream); - - try { - if (bag_) { - bag_->write(rosmsgbin.topic_name_, ros::Time(rosmsgbin.rec_time_.sec_, rosmsgbin.rec_time_.nsec_), ros_msg, - msg_defs_entry->second); - } - } catch (std::exception& e) { - WARNING("BagWriter: write fail: %s", e.what()); - return false; - } - - return true; -} - -/* ****************************************************************************************************************** */ -} // namespace bagwriter -} // namespace ros1 -} // namespace fpsdk diff --git a/fpsdk_ros1/src/utils.cpp b/fpsdk_ros1/src/utils.cpp deleted file mode 100644 index 48e8336f..00000000 --- a/fpsdk_ros1/src/utils.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 utilities - */ - -/* LIBC/STL */ -#include - -/* EXTERNAL */ -#include "fpsdk_ros1/ext/ros.hpp" - -/* Fixposition SDK */ -#include - -/* PACKAGE */ -#include "fpsdk_ros1/utils.hpp" - -namespace fpsdk { -namespace ros1 { -namespace utils { -/* ****************************************************************************************************************** */ - -using namespace fpsdk::common::logging; - -// --------------------------------------------------------------------------------------------------------------------- - -static char g_logger_name[100]; - -static void sLoggingFn(const LoggingParams& /*params*/, const LoggingLevel level, const char* str) -{ - switch (level) { // clang-format off - case LoggingLevel::TRACE: ROS_LOG(ros::console::levels::Debug, g_logger_name, "%s", str); break; - case LoggingLevel::DEBUG: ROS_LOG(ros::console::levels::Debug, g_logger_name, "%s", str); break; - case LoggingLevel::INFO: ROS_LOG(ros::console::levels::Info, g_logger_name, "%s", str); break; - case LoggingLevel::NOTICE: ROS_LOG(ros::console::levels::Info, g_logger_name, "%s", str); break; - case LoggingLevel::WARNING: ROS_LOG(ros::console::levels::Warn, g_logger_name, "%s", str); break; - case LoggingLevel::ERROR: ROS_LOG(ros::console::levels::Error, g_logger_name, "%s", str); break; - case LoggingLevel::FATAL: ROS_LOG(ros::console::levels::Fatal, g_logger_name, "%s", str); break; - } // clang-format on -} - -void RedirectLoggingToRosConsole(const char* logger_name) -{ - LoggingParams params = LoggingGetParams(); - params.fn_ = sLoggingFn; - params.level_ = LoggingLevel::TRACE; // We leave it up to ROS to decide what to print - // Set logger name. Note that ROSCONSOLE_DEFAULT_NAME here is the "ros.fpsdk_ros1" package name. However, when - // called from the app (node) then the default argument to logger_name is that package's ROSCONSOLE_DEFAULT_NAME, - // e.g. "ros.ros1_fpsdk_demo" - std::snprintf( - g_logger_name, sizeof(g_logger_name), "%s", logger_name != NULL ? logger_name : ROSCONSOLE_DEFAULT_NAME); - LoggingSetParams(params); -} - -// --------------------------------------------------------------------------------------------------------------------- - -template -static bool LoadRosParamEx(const std::string& name, T& value) -{ - try { - if (ros::param::has(name)) { - ros::param::get(name, value); - } else { - return false; - } - } catch (ros::InvalidNameException& e) { - return false; - } - return true; -} - -bool LoadRosParam(const std::string& name, int& value) -{ - return LoadRosParamEx(name, value); -} - -bool LoadRosParam(const std::string& name, std::string& value) -{ - return LoadRosParamEx(name, value); -} - -bool LoadRosParam(const std::string& name, bool& value) -{ - return LoadRosParamEx(name, value); -} - -bool LoadRosParam(const std::string& name, float& value) -{ - return LoadRosParamEx(name, value); -} - -bool LoadRosParam(const std::string& name, double& value) -{ - return LoadRosParamEx(name, value); -} - -bool LoadRosParam(const std::string& name, std::vector& value) -{ - return LoadRosParamEx(name, value); -} - -// --------------------------------------------------------------------------------------------------------------------- - -ros::Time ConvTime(const fpsdk::common::time::Time& time) -{ - const auto rt = time.GetRosTime(); - return ros::Time(rt.sec_, rt.nsec_); -} - -fpsdk::common::time::Time ConvTime(const ros::Time& time) -{ - return fpsdk::common::time::Time::FromRosTime({ time.sec, time.nsec }); -} - -/* ****************************************************************************************************************** */ -} // namespace utils -} // namespace ros1 -} // namespace fpsdk diff --git a/fpsdk_ros1/test/bagwriter_test.cpp b/fpsdk_ros1/test/bagwriter_test.cpp deleted file mode 100644 index f8ad168a..00000000 --- a/fpsdk_ros1/test/bagwriter_test.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: tests for fpsdk::ros1::bagwriter - */ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include - -/* PACKAGE */ -#include -#include - -namespace { -/* ****************************************************************************************************************** */ -using namespace fpsdk::ros1::bagwriter; - -TEST(BagwriterTest, Dummy) -{ - EXPECT_TRUE(true); -} - -/* ****************************************************************************************************************** */ -} // namespace - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - auto level = fpsdk::common::logging::LoggingLevel::WARNING; - for (int ix = 0; ix < argc; ix++) { - if ((argv[ix][0] == '-') && argv[ix][1] == 'v') { - level++; - } - } - fpsdk::common::logging::LoggingSetParams(level); - return RUN_ALL_TESTS(); -} diff --git a/fpsdk_ros1/test/utils_test.cpp b/fpsdk_ros1/test/utils_test.cpp deleted file mode 100644 index 75768890..00000000 --- a/fpsdk_ros1/test/utils_test.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: tests for fpsdk::ros1::utils - */ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include -#include - -/* PACKAGE */ -#include -#include - -namespace { -/* ****************************************************************************************************************** */ -using namespace fpsdk::ros1::utils; - -TEST(UtilsTest, ConvTime) -{ - const ros::Time ros_time(1723650283, 125000000); - DEBUG("ros::Time %" PRIu32 " %" PRIu32, ros_time.sec, ros_time.nsec); - - const fpsdk::common::time::Time sdk_time = ConvTime(ros_time); - DEBUG("SDK Time %" PRIu32 " %" PRIu32, sdk_time.sec_, sdk_time.nsec_); - EXPECT_EQ(sdk_time.sec_, ros_time.sec + 27); // 1723650310 - EXPECT_EQ(sdk_time.nsec_, ros_time.nsec); - - const ros::Time ros_time_again = ConvTime(sdk_time); - DEBUG("ros::Time %" PRIu32 " %" PRIu32, ros_time_again.sec, ros_time_again.nsec); - - EXPECT_EQ(ros_time_again.sec, ros_time.sec); - EXPECT_EQ(ros_time_again.nsec, ros_time.nsec); -} - -// --------------------------------------------------------------------------------------------------------------------- - -// This should be the last test as it messes with the console and we may want to use DEBUG() etc. in the tests -TEST(UtilsTest, RedirectLoggingToRosConsole) -{ - // Silence the ROS console - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); - - // We're not actually testing much more than checking that this doesn't crash. - // Run the test with -v -v -v and set ROS console level to Debug (above) to make it a bit more interesting in the - // output. - // clang-format off - // clear; make INSTALL_PREFIX=fpsdk BUILD_TYPE=Debug build && ROSCONSOLE_FORMAT='${severity} ${time:%Y-%m-%d %H:%M:%S.%f} ${logger} - ${message}' build/Debug/fpsdk_ros1/fpsdk_ros1_utils_test -v -v -v - // clang-format on - - ROS_DEBUG("Hello, this is a ros debug before redirect..."); - ROS_INFO("Hello, this is a ros info before redirect..."); - // ROS_WARN("Hello, this is a ros warn before redirect..."); - DEBUG("This is fpsdk_common debug before redirect..."); - INFO("This is fpsdk_common info before redirect..."); - // WARNING("This is fpsdk_common warning before redirect..."); - - RedirectLoggingToRosConsole(); - - ROS_DEBUG("Hello, this is a ros debug after redirect..."); - ROS_INFO("Hello, this is a ros info after redirect..."); - // ROS_WARN("Hello, this is a ros warn after redirect..."); - DEBUG("This is fpsdk_common debug after redirect..."); - INFO("This is fpsdk_common info after redirect..."); - // WARNING("This is fpsdk_common warning after redirect..."); -} -// This should be the last test as it messes with the console and we may want to use DEBUG() etc. in the tests - -/* ****************************************************************************************************************** */ -} // namespace - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - auto level = fpsdk::common::logging::LoggingLevel::WARNING; - for (int ix = 0; ix < argc; ix++) { - if ((argv[ix][0] == '-') && argv[ix][1] == 'v') { - level++; - } - } - fpsdk::common::logging::LoggingSetParams(level); - return RUN_ALL_TESTS(); -} diff --git a/fpsdk_ros2/CMakeLists.txt b/fpsdk_ros2/CMakeLists.txt deleted file mode 100644 index 2dfc575c..00000000 --- a/fpsdk_ros2/CMakeLists.txt +++ /dev/null @@ -1,169 +0,0 @@ -# GENERAL ============================================================================================================== -message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----") - -cmake_minimum_required(VERSION 3.16) -include(../fpsdk_common/cmake/setup.cmake) - -project(fpsdk_ros2 - LANGUAGES CXX C - VERSION ${FPSDK_VERSION_NUMBER} - DESCRIPTION "Fixposition SDK: ROS2 library" -) - - -# COMPILER SETUP ======================================================================================================= - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror \ - -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual") -set(CMAKE_CXX_FLAGS_RELEASE "-O3") -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() -if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") - add_compile_definitions(NDEBUG) -endif() - - -# DEPENDENCIES ========================================================================================================= - -# Handle this being used as add_subdirectory() of ../CMakeLists.txt vs. building this standalone -if(NOT TARGET fpsdk_common) - find_package(fpsdk_common REQUIRED) -endif() - -find_package(rclcpp REQUIRED) -find_package(rosbag2_cpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - - -# SHARED LIBRARY ======================================================================================================= - -file(GLOB CPP_FILES src/*.cpp) -add_library(${PROJECT_NAME} SHARED ${CPP_FILES}) - -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ -) - -set(FPSDK_COMMON_TRANS_DEPS "fpsdk_common;${rclcpp_TARGETS};${rosbag2_cpp_TARGETS}") - -target_link_libraries(${PROJECT_NAME} - PUBLIC - fpsdk_common - ${rclcpp_TARGETS} - ${rosbag2_cpp_TARGETS} - ${std_msgs_TARGETS} - ${sensor_msgs_TARGETS} - ${geometry_msgs_TARGETS} - ${tf2_msgs_TARGETS} - ${nav_msgs_TARGETS} - # Shouldn't this work? (see fpsdk_apps/CMakeLists.txt, examples/ros2_fpsdk_demo/CMakeLists.txt) - # std_msgs::std_msgs - # sensor_msgs::sensor_msgs - # geometry_msgs::geometry_msgs - # tf2_msgs::tf2_msgs - # nav_msgs::nav_msgs - PRIVATE -) - -set_target_properties(${PROJECT_NAME} - PROPERTIES - VERSION ${PROJECT_VERSION} -) - - -# INSTALL ============================================================================================================== - -include(GNUInstallDirs) # Provides nice relative paths wrt CMAKE_INSTALL_PREFIX -set(PROJECT_RUNTIME_DIR ${CMAKE_INSTALL_FULL_BINDIR}) -set(PROJECT_LIBRARY_DIR ${CMAKE_INSTALL_FULL_LIBDIR}) -set(PROJECT_INCLUDE_DIR ${CMAKE_INSTALL_FULL_INCLUDEDIR}/${PROJECT_NAME}) -set(PROJECT_DATA_DIR ${CMAKE_INSTALL_FULL_DATAROOTDIR}/${PROJECT_NAME}) -set(PROJECT_DOC_DIR ${CMAKE_INSTALL_FULL_DOCDIR}) - -# Headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${PROJECT_INCLUDE_DIR} -) - -# Library, tools -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}-targets - LIBRARY DESTINATION ${PROJECT_LIBRARY_DIR} - RUNTIME DESTINATION ${PROJECT_RUNTIME_DIR} -) - -# CMake target config -install(EXPORT ${PROJECT_NAME}-targets - NAMESPACE ${PROJECT_NAMESPACE_PREFIX} - FILE ${PROJECT_NAME}-targets.cmake - DESTINATION lib/cmake/${PROJECT_NAME} -) - -# CMake config -include(CMakePackageConfigHelpers) -set(TARGET1 ${PROJECT_NAME}) -configure_package_config_file( - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/config.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake - INSTALL_DESTINATION lib/cmake/${PROJECT_NAME} - NO_SET_AND_CHECK_MACRO - NO_CHECK_REQUIRED_COMPONENTS_MACRO -) -write_basic_package_version_file( - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake - VERSION "${CMAKE_PROJECT_VERSION_MAJOR}.${CMAKE_PROJECT_VERSION_MINOR}" - COMPATIBILITY AnyNewerVersion -) -install( - FILES - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake - DESTINATION lib/cmake/${PROJECT_NAME} -) - -# pkg-config config -include(CMakePackageConfigHelpers) -set(TARGET1 ${PROJECT_NAME}) -configure_package_config_file( - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/config.pc.in - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc - INSTALL_DESTINATION lib/cmake/${PROJECT_NAME} - NO_SET_AND_CHECK_MACRO - NO_CHECK_REQUIRED_COMPONENTS_MACRO -) -install( - FILES - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc - DESTINATION lib/pkgconfig -) - -# Documentation -install( - FILES - ${PROJECT_SOURCE_DIR}/README.md - ${PROJECT_SOURCE_DIR}/LICENSE - DESTINATION ${PROJECT_DOC_DIR} -) - - -# TESTS ================================================================================================================ - -include(../fpsdk_common/cmake/testing.cmake) - -add_gtest(TARGET bagwriter_test SOURCES test/bagwriter_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS} ${rosbag2_cpp_TARGETS}) -add_gtest(TARGET utils_test SOURCES test/utils_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS}) -add_gtest(TARGET ros1_test SOURCES test/ros1_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS} ${nav_msgs_TARGETS} ${sensor_msgs_TARGETS} ${tf2_msgs_TARGETS} ${std_msgs_TARGETS}) - - -# ====================================================================================================================== diff --git a/fpsdk_ros2/LICENSE b/fpsdk_ros2/LICENSE deleted file mode 100644 index 52b9c765..00000000 --- a/fpsdk_ros2/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) Fixposition AG (www.fixposition.com) and 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. diff --git a/fpsdk_ros2/README.md b/fpsdk_ros2/README.md deleted file mode 100644 index a13685da..00000000 --- a/fpsdk_ros2/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# Fixposition SDK: ROS2 Library - -See the [../README.md](../README.md) for details. - -License: see the [LICENSE](LICENSE) file and [../README.md#license](../README.md#license). diff --git a/fpsdk_ros2/cmake/config.cmake.in b/fpsdk_ros2/cmake/config.cmake.in deleted file mode 100644 index 9463d461..00000000 --- a/fpsdk_ros2/cmake/config.cmake.in +++ /dev/null @@ -1,19 +0,0 @@ -@PACKAGE_INIT@ - -include ( "${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake" ) - -# library version information -set(@PROJECT_NAME@_VERSION_STRING "@PROJECT_VERSION@") -set(@PROJECT_NAME@_VERSION_MAJOR @PROJECT_VERSION_MAJOR@) -set(@PROJECT_NAME@_VERSION_MINOR @PROJECT_VERSION_MINOR@) -set(@PROJECT_NAME@_VERSION_PATCH @PROJECT_VERSION_PATCH@) - -set(@PROJECT_NAME@_FOUND 1) - -# Include and library dirs -set(_INSTALL_PREFIX "@CMAKE_INSTALL_PREFIX@") -set(@PROJECT_NAME@_INCLUDE_DIRS "${_INSTALL_PREFIX}/@CMAKE_INSTALL_INCLUDEDIR@") -set(@PROJECT_NAME@_LIBRARY_DIRS "${_INSTALL_PREFIX}/@CMAKE_INSTALL_LIBDIR@") - -# Libraries -set(@PROJECT_NAME@_LIBRARIES "@TARGET1@") diff --git a/fpsdk_ros2/cmake/config.pc.in b/fpsdk_ros2/cmake/config.pc.in deleted file mode 100644 index 3cc02c12..00000000 --- a/fpsdk_ros2/cmake/config.pc.in +++ /dev/null @@ -1,12 +0,0 @@ -prefix=@CMAKE_INSTALL_PREFIX@ -exec_prefix=@CMAKE_INSTALL_PREFIX@ -libdir=${exec_prefix}/@CMAKE_INSTALL_LIBDIR@ -includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ - -Name: @PROJECT_NAME@ -Description: @PROJECT_DESCRIPTION@ -Version: @PROJECT_VERSION@ - -Requires: -Libs: -L${libdir} -l@TARGET1@ -Cflags: -I${includedir} diff --git a/fpsdk_ros2/doc/doc.hpp b/fpsdk_ros2/doc/doc.hpp deleted file mode 100644 index 1522543b..00000000 --- a/fpsdk_ros2/doc/doc.hpp +++ /dev/null @@ -1,48 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 library documentation - */ - -#ifndef __FPSDK_ROS2_DOC_HPP__ -#define __FPSDK_ROS2_DOC_HPP__ - -namespace fpsdk { -/** - * @brief Fixposition SDK: ROS2 library - */ -namespace ros2 { -// clang-format off -/* ****************************************************************************************************************** */ - -/*! - @page FPSDK_ROS2_DOC Fixposition SDK: ROS2 Library - - @section FPSDK_ROS2_OVERVIEW Overview - - @todo add documentation - - @section FPSDK_ROS2_MODULES Modules - - - @subpage FPSDK_ROS2_BAGWRITER - - @subpage FPSDK_ROS2_UTILS - - @subpage FPSDK_ROS2_ROS1 - - @section FPSDK_ROS2_LICENSE License - - @include fpsdk_ros1/LICENSE - -*/ - -/* ****************************************************************************************************************** */ -// clang-format on -} // namespace ros2 -} // namespace fpsdk -#endif // __FPSDK_ROS1_DOC_HPP__ diff --git a/fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp b/fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp deleted file mode 100644 index 4713c01f..00000000 --- a/fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp +++ /dev/null @@ -1,133 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 bag writer - * - * @page FPSDK_ROS2_BAGWRITER ROS2 bag writer - * - * **API**: fpsdk_ros1/bagwriter.hpp and fpsdk::ros1::bagwriter - * - */ -#ifndef __FPSDK_ROS2_BAGWRITER_HPP__ -#define __FPSDK_ROS2_BAGWRITER_HPP__ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include "fpsdk_ros2/ext/rosbag2_cpp_writer.hpp" - -/* Fixposition SDK */ -#include -#include - -/* PACKAGE */ - -namespace fpsdk { -namespace ros2 { -/** - * @brief ROS2 bag writer - */ -namespace bagwriter { -/* ****************************************************************************************************************** */ - -/** - * @brief ROS2 bag writer helper - */ -class BagWriter -{ - public: - BagWriter(); - ~BagWriter(); - - /** - * @brief Open bag for writing - * - * @param[in] path Path of the bag directory - * @param[in] compress Compress bag, 0 = no compression, 1 = ... - * Note: not fully implemented - * - * @returns true if bag was sucessfully opened - */ - bool Open(const std::string& path, const int compress = 0); - - /** - * @brief Close bag - */ - void Close(); - - /** - * @brief Write a message to the bag - * - * @tparam T ROS message type - * @param[in] msg The message - * @param[in] topic Topic name - * @param[in] time Bag record time - * - * @returns true if message was added, false otherwise (message definition missing) - */ - template - bool WriteMessage(const T& msg, const std::string& topic, const rclcpp::Time& time) - { - bool ok = false; - try { - if (bag_) { - bag_->write(msg, topic, time); - ok = true; - } - } catch (const std::exception& ex) { - WARNING("BagWriter: write fail: %s", ex.what()); - } - return ok; - } - - /** - * @brief Write a message to the bag - * - * @tparam T ROS message type - * @param[in] msg The message - * @param[in] topic Topic name - * @param[in] time Bag record time - */ - template - bool WriteMessage(const T& msg, const std::string& topic, const common::time::RosTime& time) - { - return WriteMessage(msg, topic, rclcpp::Time(time.sec_, time.nsec_, RCL_ROS_TIME)); - } - - /** - * @brief Add ROS message definition from .fpl - * - * @note No checks on the provided data are done! - * - * @param[in] rosmsgdef The message definition - */ - void AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef); - - /** - * @brief Write message from .fpl - * - * @note No checks on the provided data are done! - * - * @param[in] rosmsgbin The recorded message - * - * @returns true if message was added, false otherwise (e.g. ROS1->ROS2 conversion not implemented) - */ - bool WriteMessage(const common::fpl::RosMsgBin& rosmsgbin); - - private: - std::unique_ptr bag_; //!< Bag file handle - std::map defs_; //!< Message definitions (connection headers) -}; - -/* ****************************************************************************************************************** */ -} // namespace bagwriter -} // namespace ros2 -} // namespace fpsdk -#endif // __FPSDK_ROS2_BAGWRITER_HPP__ diff --git a/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp b/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp deleted file mode 100644 index 4dcf4df0..00000000 --- a/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp +++ /dev/null @@ -1,17 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS2_EXT_MSGS_HPP__ -#define __FPSDK_ROS2_EXT_MSGS_HPP__ -#pragma GCC diagnostic push -// #pragma GCC diagnostic ignored "-Wpedantic" -// #pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" - -#include -#include -#include -#include -#include -#include - -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS2_EXT_MSGS_HPP__ diff --git a/fpsdk_ros2/include/fpsdk_ros2/ext/rclcpp.hpp b/fpsdk_ros2/include/fpsdk_ros2/ext/rclcpp.hpp deleted file mode 100644 index bf982a4a..00000000 --- a/fpsdk_ros2/include/fpsdk_ros2/ext/rclcpp.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS2_EXT_RCLCPP_HPP__ -#define __FPSDK_ROS2_EXT_RCLCPP_HPP__ -#pragma GCC diagnostic push -// #pragma GCC diagnostic ignored "-Wpedantic" -// #pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS2_EXT_RCLCPP_HPP__ diff --git a/fpsdk_ros2/include/fpsdk_ros2/ext/rosbag2_cpp_writer.hpp b/fpsdk_ros2/include/fpsdk_ros2/ext/rosbag2_cpp_writer.hpp deleted file mode 100644 index f7d9cb10..00000000 --- a/fpsdk_ros2/include/fpsdk_ros2/ext/rosbag2_cpp_writer.hpp +++ /dev/null @@ -1,11 +0,0 @@ -// Wrapper to suppress warnings from ROS headers -#ifndef __FPSDK_ROS2_EXT_ROSBAG2_CPP_WRITER_HPP__ -#define __FPSDK_ROS2_EXT_ROSBAG2_CPP_WRITER_HPP__ -#pragma GCC diagnostic push -// #pragma GCC diagnostic ignored "-Wpedantic" -// #pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#include -#pragma GCC diagnostic pop -#endif // __FPSDK_ROS2_EXT_ROSBAG2_CPP_WRITER_HPP__ diff --git a/fpsdk_ros2/include/fpsdk_ros2/utils.hpp b/fpsdk_ros2/include/fpsdk_ros2/utils.hpp deleted file mode 100644 index 60be24bb..00000000 --- a/fpsdk_ros2/include/fpsdk_ros2/utils.hpp +++ /dev/null @@ -1,80 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 utilities - * - * @page FPSDK_ROS2_UTILS ROS2 utilities - * - * **API**: fpsdk_ros2/utils.hpp and fpsdk::ros2::utils - * - */ -#ifndef __FPSDK_ROS2_UTILS_HPP__ -#define __FPSDK_ROS2_UTILS_HPP__ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include - -/* Fixposition SDK */ -#include - -/* PACKAGE */ - -namespace fpsdk { -namespace ros2 { -/** - * @brief ROS2 utilities - */ -namespace utils { -/* ****************************************************************************************************************** */ - -/** - * @brief Redirect fp:common::logging to ROS console - * - * This configures the fpsdk::common::logging facility to output via the ROS console. This does *not* configure the ROS - * console (logger level, logger name, etc.). - * - * The mapping of fpsdk::common::logging::LoggingLevel to rclcpp levels is as follows: - * - * - TRACE and DEBUG --> DEBUG - * - INFO and NOTICE --> INFO - * - WARNING --> WARN - * - ERROR --> ERROR - * - FATAL --> FATAL - * - * @param[in] logger_name The name of the logger. The recommended value is node->get_logger().get_name() - */ -void RedirectLoggingToRosConsole(const char* logger_name = "fpsdk_ros2"); - -/** - * @brief Convert to ROS time (atomic -> POSIX) - * - * @param[in] time The Time object (atomic) - * @param[in] clock_type The clock to use (to assume) - * - * @returns the ROS time object (POSIX) - */ -rclcpp::Time ConvTime(const fpsdk::common::time::Time& time, rcl_clock_type_t clock_type = RCL_ROS_TIME); - -/** - * @brief Convert from ROS time (POSIX -> atomic) - * - * @param[in] time The ROS time object (POSIX) - * - * @returns the Time object (atomic) - */ -fpsdk::common::time::Time ConvTime(const rclcpp::Time& time); - -/* ****************************************************************************************************************** */ -} // namespace utils -} // namespace ros2 -} // namespace fpsdk -#endif // __FPSDK_ROS2_UTILS_HPP__ diff --git a/fpsdk_ros2/package.xml b/fpsdk_ros2/package.xml deleted file mode 100644 index 9a83aabd..00000000 --- a/fpsdk_ros2/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - fpsdk_ros2 - 1.0.0 - Fixposition SDK: ROS2 library - Fixposition Support - MIT - - cmake - - fpsdk_common - std_msgs - sensor_msgs - geometry_msgs - tf2_msgs - rosbag2_cpp - diff --git a/fpsdk_ros2/src/ros1.cpp b/fpsdk_ros2/src/ros1.cpp deleted file mode 100644 index bc36e20f..00000000 --- a/fpsdk_ros2/src/ros1.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 types conversion from ROS1 - */ - -/* LIBC/STL */ - -/* EXTERNAL */ - -/* Fixposition SDK */ -#include - -/* PACKAGE */ -#include "fpsdk_ros2/ros1.hpp" - -namespace fpsdk { -namespace ros2 { -namespace ros1 { -/* ****************************************************************************************************************** */ - -// --------------------------------------------------------------------------------------------------------------------- - -/* ****************************************************************************************************************** */ -} // namespace ros1 -} // namespace ros2 -} // namespace fpsdk diff --git a/fpsdk_ros2/src/utils.cpp b/fpsdk_ros2/src/utils.cpp deleted file mode 100644 index 032dc703..00000000 --- a/fpsdk_ros2/src/utils.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS2 utilities - */ - -/* LIBC/STL */ -#include - -/* EXTERNAL */ - -/* Fixposition SDK */ -#include - -/* PACKAGE */ -#include "fpsdk_ros2/utils.hpp" - -namespace fpsdk { -namespace ros2 { -namespace utils { -/* ****************************************************************************************************************** */ - -using namespace fpsdk::common::logging; - -// --------------------------------------------------------------------------------------------------------------------- - -static std::unique_ptr g_logger; - -static void sLoggingFn(const LoggingParams& /*params*/, const LoggingLevel level, const char* str) -{ - // These will appear under the "fpsdk_ros2" logger. - switch (level) { // clang-format off - case LoggingLevel::TRACE: RCLCPP_DEBUG((*g_logger), "%s", str); break; - case LoggingLevel::DEBUG: RCLCPP_DEBUG((*g_logger), "%s", str); break; - case LoggingLevel::INFO: RCLCPP_INFO( (*g_logger), "%s", str); break; - case LoggingLevel::NOTICE: RCLCPP_INFO( (*g_logger), "%s", str); break; - case LoggingLevel::WARNING: RCLCPP_WARN( (*g_logger), "%s", str); break; - case LoggingLevel::ERROR: RCLCPP_ERROR((*g_logger), "%s", str); break; - case LoggingLevel::FATAL: RCLCPP_FATAL((*g_logger), "%s", str); break; - } // clang-format on -} - -void RedirectLoggingToRosConsole(const char* logger_name) -{ - g_logger = std::make_unique(rclcpp::get_logger(logger_name)); - LoggingParams params = LoggingGetParams(); - params.fn_ = sLoggingFn; - params.level_ = LoggingLevel::TRACE; // We leave it up to ROS to decide what to print - LoggingSetParams(params); -} - -// --------------------------------------------------------------------------------------------------------------------- - -rclcpp::Time ConvTime(const fpsdk::common::time::Time& time, rcl_clock_type_t clock_type) -{ - const auto rt = time.GetRosTime(); - return rclcpp::Time(rt.sec_, rt.nsec_, clock_type); -} - -fpsdk::common::time::Time ConvTime(const rclcpp::Time& time) -{ - const uint64_t nsec = time.nanoseconds(); - return fpsdk::common::time::Time::FromRosTime( - { static_cast(nsec / 1000000000), static_cast(nsec % 1000000000) }); -} - -/* ****************************************************************************************************************** */ -} // namespace utils -} // namespace ros2 -} // namespace fpsdk diff --git a/fpsdk_ros2/test/bagwriter_test.cpp b/fpsdk_ros2/test/bagwriter_test.cpp deleted file mode 100644 index fc906112..00000000 --- a/fpsdk_ros2/test/bagwriter_test.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: tests for fpsdk::ros2::bagwriter - */ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include - -/* PACKAGE */ -#include -#include - -namespace { -/* ****************************************************************************************************************** */ -using namespace fpsdk::ros2::bagwriter; - -TEST(BagwriterTest, Dummy) -{ - EXPECT_TRUE(true); -} - -/* ****************************************************************************************************************** */ -} // namespace - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - auto level = fpsdk::common::logging::LoggingLevel::WARNING; - for (int ix = 0; ix < argc; ix++) { - if ((argv[ix][0] == '-') && argv[ix][1] == 'v') { - level++; - } - } - fpsdk::common::logging::LoggingSetParams(level); - return RUN_ALL_TESTS(); -} diff --git a/fpsdk_ros2/test/ros1_test.cpp b/fpsdk_ros2/test/ros1_test.cpp deleted file mode 100644 index 0dfc0544..00000000 --- a/fpsdk_ros2/test/ros1_test.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: tests for fpsdk::ros2::ros1 - */ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include - -/* PACKAGE */ -#include -#include - -namespace { -/* ****************************************************************************************************************** */ -using namespace fpsdk::ros2::ros1; - -TEST(Ros1Test, Dummy) -{ - EXPECT_TRUE(true); -} - -/* ****************************************************************************************************************** */ -} // namespace - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - auto level = fpsdk::common::logging::LoggingLevel::WARNING; - for (int ix = 0; ix < argc; ix++) { - if ((argv[ix][0] == '-') && argv[ix][1] == 'v') { - level++; - } - } - fpsdk::common::logging::LoggingSetParams(level); - return RUN_ALL_TESTS(); -}