Extract more data from .fpl recordings, and related cleanup/refactor

.devcontainer/.gdb/gdbinit

@@ -10,10 +10,10 @@ sys.path.insert(0, '/home/fpsdk/fixposition-sdk/.devcontainer/.gdb')
 sys.path.insert(0, os.getcwd() + '/.devcontainer/.gdb')
 #/usr/share/gcc/python/libstdcxx
 
-# Natvis
-print('- loading natvis4gdb')
-import natvis4gdb
-natvis4gdb.register()
+# # Natvis
+# print('- loading natvis4gdb')
+# import natvis4gdb
+# natvis4gdb.register()
 
 # STL pretty printer
 print('- loading libstdcxx pretty printer')
@@ -24,7 +24,7 @@ register_libstdcxx_printers(None)
 # print('- loading GLM pretty printer')
 # import glm_pp
 
-# JSNO pretty printer
+# JSON pretty printer
 print('- loading json pretty printer')
 import nlohmann_json
 

.gitignore

@@ -31,6 +31,15 @@ foo*
 bar*
 baz*
 /tmp
+/*.raw
+/*.fpl
+/*.jsonl
+/*.raw.gz
+/*.fpl.gz
+/*.jsonl.gz
+/*.yaml
+/*.gz
+/*.json
 
 # We must not commit such files ever. See section FPSDK_BUILD_ROS in fpsdk_doc/fpsdk_build.hpp
 CATKIN_IGNORE

Makefile

@@ -175,7 +175,7 @@ ifneq ($(MAKECMDGOALS),distclean)
 ifneq ($(MAKECMDGOALS),doc)
 builddiruid=$(shell $(CAT) $(BUILD_DIR)/.make-uid 2>/dev/null || echo "none")
 ifneq ($(builddiruid),$(configuid))
-    $(shell $(RM) -f $(BUILD_DIR)/.make-uid)
+    dummy=$(shell $(RM) -f $(BUILD_DIR)/.make-uid)
 endif
 endif
 endif

docker/Dockerfile.humble-dev

@@ -24,10 +24,10 @@ RUN /tmp/unminimize_system.sh && rm -f /tmp/unminimize_system.sh
 COPY docker/scripts/adduser_fpsdk.sh /tmp
 RUN /tmp/adduser_fpsdk.sh && rm -f /tmp/adduser_fpsdk.sh
 
-# Install pre-commit hooks (fpsdk user)
-COPY .pre-commit-config.yaml /tmp
-COPY docker/scripts/install_precommithooks.sh /tmp
-RUN sudo -u fpsdk /tmp/install_precommithooks.sh && rm -f /tmp/install_precommithooks.sh /tmp/.pre-commit-config.yaml
+# # Install pre-commit hooks (fpsdk user)
+# COPY .pre-commit-config.yaml /tmp
+# COPY docker/scripts/install_precommithooks.sh /tmp
+# RUN sudo -u fpsdk /tmp/install_precommithooks.sh && rm -f /tmp/install_precommithooks.sh /tmp/.pre-commit-config.yaml
 
 # Install git-bash-prompt (fpsdk user)
 COPY .devcontainer/fpsdk.bgptheme /tmp

docker/Dockerfile.jazzy-dev

@@ -24,10 +24,10 @@ RUN /tmp/unminimize_system.sh && rm -f /tmp/unminimize_system.sh
 COPY docker/scripts/adduser_fpsdk.sh /tmp
 RUN /tmp/adduser_fpsdk.sh && rm -f /tmp/adduser_fpsdk.sh
 
-# Install pre-commit hooks (fpsdk user)
-COPY .pre-commit-config.yaml /tmp
-COPY docker/scripts/install_precommithooks.sh /tmp
-RUN sudo -u fpsdk /tmp/install_precommithooks.sh && rm -f /tmp/install_precommithooks.sh /tmp/.pre-commit-config.yaml
+# # Install pre-commit hooks (fpsdk user)
+# COPY .pre-commit-config.yaml /tmp
+# COPY docker/scripts/install_precommithooks.sh /tmp
+# RUN sudo -u fpsdk /tmp/install_precommithooks.sh && rm -f /tmp/install_precommithooks.sh /tmp/.pre-commit-config.yaml
 
 # Install git-bash-prompt (fpsdk user)
 COPY .devcontainer/fpsdk.bgptheme /tmp

docker/Dockerfile.noetic-dev

@@ -22,10 +22,10 @@ RUN /tmp/unminimize_system.sh && rm -f /tmp/unminimize_system.sh
 COPY docker/scripts/adduser_fpsdk.sh /tmp
 RUN /tmp/adduser_fpsdk.sh && rm -f /tmp/adduser_fpsdk.sh
 
-# Install pre-commit hooks (fpsdk user)
-COPY .pre-commit-config.yaml /tmp
-COPY docker/scripts/install_precommithooks.sh /tmp
-RUN sudo -u fpsdk /tmp/install_precommithooks.sh && rm -f /tmp/install_precommithooks.sh /tmp/.pre-commit-config.yaml
+# # Install pre-commit hooks (fpsdk user)
+# COPY .pre-commit-config.yaml /tmp
+# COPY docker/scripts/install_precommithooks.sh /tmp
+# RUN sudo -u fpsdk /tmp/install_precommithooks.sh && rm -f /tmp/install_precommithooks.sh /tmp/.pre-commit-config.yaml
 
 # Install git-bash-prompt (fpsdk user)
 COPY .devcontainer/fpsdk.bgptheme /tmp

docker/ci.sh

@@ -474,12 +474,11 @@ function build_projs_release_ros2
     cmake --build build/${buildname}/fpsdk_common ${CMAKE_BUILD_ARGS} || return 1
     cmake --install build/${buildname}/fpsdk_common || return 1
 
-    # TODO: not working yet
-    # 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_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} \

docker/scripts/install_apt_base.sh

@@ -30,6 +30,7 @@ packages=$(awk -v filt=${FPSDK_IMAGE%-*} '$1 ~ filt { print $2 }' <<EOF
     noetic.humble.jazzy.trixie      gnuplot
     ..............jazzy.trixie      googletest
     noetic.humble.jazzy.trixie      graphviz
+    noetic.humble.jazzy.trixie      jq
     ....................trixie      libbacktrace-dev                  # GCC has its own, but clang needs this
     noetic.humble.jazzy.trixie      libboost-all-dev                  # This is not small... :-/
     noetic.humble.jazzy.trixie      libclone-perl
@@ -55,6 +56,8 @@ packages=$(awk -v filt=${FPSDK_IMAGE%-*} '$1 ~ filt { print $2 }' <<EOF
     noetic.humble.jazzy.trixie      python3-osrf-pycommon
     noetic.humble.jazzy.trixie      python3-pip
     noetic.humble.jazzy.trixie      python3-venv
+    .......humble.............      ros-humble-rosbag2-storage-mcap
+    ..............jazzy.......      ros-jazzy-rosbag2-storage-mcap
     noetic....................      ros-noetic-eigen-conversions
     noetic....................      ros-noetic-tf
     noetic....................      ros-noetic-tf-conversions

examples/ros1_fpsdk_demo/CMakeLists.txt

@@ -1,4 +1,5 @@
 # GENERAL ==============================================================================================================
+message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----")
 
 cmake_minimum_required(VERSION 3.16)
 
@@ -42,8 +43,6 @@ catkin_package(
     std_msgs
 )
 
-include_directories(include ${catkin_INCLUDE_DIRS} ${fpsdk_common_INCLUDE_DIRS} ${fpsdk_ros1_INCLUDE_DIRS})
-
 
 # SHARED LIBRARY =======================================================================================================
 
@@ -53,6 +52,12 @@ add_library(${PROJECT_NAME}_lib SHARED
 
 target_link_libraries(${PROJECT_NAME}_lib ${catkin_LIBRARIES} fpsdk_common fpsdk_ros1)
 
+target_include_directories(${PROJECT_NAME}_lib
+    PUBLIC
+        $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+        ${catkin_INCLUDE_DIRS}
+)
+
 
 # EXECUTABLES ==========================================================================================================
 

examples/ros2_fpsdk_demo/CMakeLists.txt

@@ -1,4 +1,5 @@
 # GENERAL ==============================================================================================================
+message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----")
 
 cmake_minimum_required(VERSION 3.16)
 
@@ -32,8 +33,9 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(fpsdk_common REQUIRED)
 find_package(fpsdk_ros2 REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
 
-include_directories(include ${rclcpp_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ${fpsdk_common_INCLUDE_DIRS} ${fpsdk_ros2_INCLUDE_DIRS})
+# include_directories(include ${rclcpp_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ${fpsdk_common_INCLUDE_DIRS})
 
 
 # SHARED LIBRARY =======================================================================================================
@@ -52,9 +54,14 @@ add_executable(${PROJECT_NAME}_node
     src/node_main.cpp
 )
 
-target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_lib fpsdk_common fpsdk_ros2)
+target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_lib fpsdk_common fpsdk_ros2 ${rosbag2_cpp_TARGETS})
 ament_target_dependencies(${PROJECT_NAME}_node rclcpp fpsdk_common fpsdk_ros2)
 
+target_include_directories(${PROJECT_NAME}_lib
+    PUBLIC
+        $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+)
+
 install(TARGETS
     ${PROJECT_NAME}_lib ${PROJECT_NAME}_node
     LIBRARY DESTINATION lib

fpsdk.code-workspace

@@ -221,7 +221,15 @@
             "filesystem": "cpp"
         },
         "workbench.remoteIndicator.showExtensionRecommendations": true,
-        "yaml.extension.recommendations": false
+        "yaml.extension.recommendations": false,
+        "workbench.colorCustomizations": {
+            "commandCenter.border": "#15202b99",
+            "titleBar.activeBackground": "#f9e64f",
+            "titleBar.activeForeground": "#15202b",
+            "titleBar.inactiveBackground": "#f9e64f99",
+            "titleBar.inactiveForeground": "#15202b99"
+        },
+        "peacock.remoteColor": "#f9e64f"
     },
     "extensions": {
         "recommendations": [
@@ -295,16 +303,7 @@
                 "stopAtEntry": true,
                 "preLaunchTask" : "Fixposition SDK: build",
                 "program": "${workspaceFolder}/build/Debug/fpsdk_apps/fpltool",
-                "args": [ "-v", "-v" ],
-                "cwd": "${workspaceFolder}"
-            },
-            {
-                "name": "Fixposition SDK: debug trafo_test",
-                "type": "cppdbg", "request": "launch", "MIMode": "gdb", "miDebuggerPath" : "gdb",
-                "stopAtEntry": true,
-                "preLaunchTask" : "Fixposition SDK: build",
-                "program": "${workspaceFolder}/build/Debug/fpsdk_common/fpsdk_common_trafo_test",
-                "args": [ "-v", "-v", "-v", "-v" ],
+                "args": [ "-v", "extract", "filedumpworking.fpl", "-f" ],
                 "cwd": "${workspaceFolder}",
                 "setupCommands": [
                     { "text": "set auto-load safe-path ${workspaceFolder}", "description": "", "ignoreFailures": true },
@@ -314,27 +313,12 @@
                 ]
             },
             {
-                "name": "Fixposition SDK: debug streamtool",
+                "name": "Fixposition SDK: debug trafo_test",
                 "type": "cppdbg", "request": "launch", "MIMode": "gdb", "miDebuggerPath" : "gdb",
                 "stopAtEntry": true,
                 "preLaunchTask" : "Fixposition SDK: build",
-                "program": "${workspaceFolder}/build/Debug/fpsdk_apps/streamtool",
-                "args": [ "-v", "-v", "-v", "tcpsvr://:12345" ],
-                "cwd": "${workspaceFolder}",
-                "setupCommands": [
-                    { "text": "set auto-load safe-path ${workspaceFolder}", "description": "", "ignoreFailures": true },
-                    { "text": "-enable-pretty-printing", "description": "enable pretty printing", "ignoreFailures": true },
-                    { "text": "handle SIGPIPE nostop noprint pass", "description": "ignore SIGPIPE", "ignoreFailures": true },
-                    { "text": "source ${workspaceFolder}/.devcontainer/.gdb/gdbinit", "description": "load local gdbinit", "ignoreFailures": false }
-                ]
-            },
-            {
-                "name": "Fixposition SDK: debug streammux",
-                "type": "cppdbg", "request": "launch", "MIMode": "gdb", "miDebuggerPath" : "gdb",
-                "stopAtEntry": false,
-                "preLaunchTask" : "Fixposition SDK: build",
-                "program": "${workspaceFolder}/build/Debug/fpsdk_apps/streammux",
-                "args": [ "-v", "-v", "-v", "-s", "tcpsvr://:10001", "-s", "tcpsvr://:10002", "-m", "1=2", "-a", ":20000/meier" ],
+                "program": "${workspaceFolder}/build/Debug/fpsdk_common/fpsdk_common_trafo_test",
+                "args": [ "-v", "-v", "-v", "-v" ],
                 "cwd": "${workspaceFolder}",
                 "setupCommands": [
                     { "text": "set auto-load safe-path ${workspaceFolder}", "description": "", "ignoreFailures": true },

fpsdk.sh

@@ -106,16 +106,22 @@ function main
 
     debug "SCRIPTDIR=${SCRIPTDIR} image=${image} docker_args=${docker_args} volume_args=${volume_args} have_command=${have_command} command=$@"
 
-    # Check that script is run as user and docker is setup properly
-    if ! which docker >/dev/null; then
-        exit_fail "Docker does not seem to be installed here"
+    # Check if we're running inside a container
+    # PID 2 will be kthreadd on host Linux, and no kthreadd will be visible in a container
+    if ! grep -q kthreadd /proc/2/status 2>/dev/null; then
+        error "You cannot run this in a container"
+        exit 1
+    fi
+
+    # Check for common issues
+    if ! command -v docker >/dev/null ]; then
+        warning "Docker does not seem to be installed, this probably doesn't work"
     fi
     if [ $(id -u) -eq 0 ]; then
-        exit_fail "This script should not be run as root"
-        exit 1
+        warning "You probably should not run this as root"
     fi
     if ! id -nG | grep -qw docker; then
-        exit_fail "User $USER is not in the docker group"
+        echo "You're not in the docker group, this may not work"
     fi
 
     local res=0

fpsdk_apps/CMakeLists.txt

@@ -1,7 +1,9 @@
 # GENERAL ==============================================================================================================
+message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----")
 
 cmake_minimum_required(VERSION 3.16)
 include(../fpsdk_common/cmake/setup.cmake)
+include(../fpsdk_common/cmake/ros.cmake)
 
 project(fpsdk_apps
     LANGUAGES CXX
@@ -37,26 +39,26 @@ endif()
 if(FPSDK_USE_ROS1)
     if(NOT TARGET fpsdk_ros1)
         find_package(fpsdk_ros1 REQUIRED)
-        message(STATUS "fpsdk_apps: fpsdk_ros1_INCLUDE_DIRS=${fpsdk_ros1_INCLUDE_DIRS}")
-        message(STATUS "fpsdk_apps: fpsdk_ros1_LIBRARIES=${fpsdk_ros1_LIBRARIES}")
     endif()
+    fpsdk_cmake_find_ros1_package(rosbag)
     add_compile_definitions(FPSDK_USE_ROS1)
 elseif(FPSDK_USE_ROS2)
     add_compile_definitions(FPSDK_USE_ROS2)
+    find_package(rosbag2_cpp REQUIRED)
+    if(NOT TARGET fpsdk_ros2)
+        find_package(fpsdk_ros2 REQUIRED)
+    endif()
 else()
     message(STATUS "fpsdk_apps: No ROS available")
 endif()
 
-include_directories(include ${fpsdk_ros1_INCLUDE_DIRS})
-
-
 # SHARED LIBRARY =======================================================================================================
 
 # 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})
+    #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)
@@ -73,6 +75,9 @@ target_link_libraries(fpltool
     PRIVATE
         fpsdk_common
         $<$<BOOL:${FPSDK_USE_ROS1}>:fpsdk_ros1>
+        $<$<BOOL:${FPSDK_USE_ROS2}>:fpsdk_ros2>
+        $<TARGET_NAME_IF_EXISTS:ros1::rosbag>
+        ${rosbag2_cpp_TARGETS}
 )
 
 file(GLOB PARSERTOOL_CPP_FILES parsertool/*.cpp)

fpsdk_apps/fpltool/fpltool.cpp

@@ -26,7 +26,6 @@
 #include "fpltool_meta.hpp"
 #include "fpltool_opts.hpp"
 #include "fpltool_record.hpp"
-#include "fpltool_rosbag.hpp"
 #include "fpltool_trim.hpp"
 
 /* ****************************************************************************************************************** */
@@ -51,10 +50,15 @@ int main(int argc, char** argv)
         switch (opts.command_) { /* clang-format off */
             case FplToolOptions::Command::DUMP:        ok = DoDump(opts);    break;
             case FplToolOptions::Command::META:        ok = DoMeta(opts);    break;
-            case FplToolOptions::Command::ROSBAG:      ok = DoRosbag(opts);  break;
             case FplToolOptions::Command::TRIM:        ok = DoTrim(opts);    break;
-            case FplToolOptions::Command::RECORD:      ok = DoRecord(opts);  break;
             case FplToolOptions::Command::EXTRACT:     ok = DoExtract(opts); break;
+            // Aliases
+            case FplToolOptions::Command::ROSBAG:
+                opts.formats_.push_back(opts.FORMAT_ROS);
+                ok = DoExtract(opts);
+                break;
+            // Not implemented, invalid
+            case FplToolOptions::Command::RECORD:      ok = DoRecord(opts);  break;
             case FplToolOptions::Command::UNSPECIFIED: ok = false;           break;
         }  // clang-format on
     }

fpsdk_apps/fpltool/fpltool_doc.hpp

@@ -24,20 +24,21 @@ namespace fpltool {
 /* ****************************************************************************************************************** */
 
 /*!
-    @page FPSDK_APPS_FPLTOOL .fpl logfile tool
+    @page FPSDK_APPS_FPLTOOL .fpl tool
 
     @section FPSDK_APPS_FPLTOOL_OVERVIEW Overview
 
-    Tool to process .fpl logfiles (recordings). This demonstrates the use of the @ref FPSDK_COMMON_FPL
+    Tool to process .fpl files. This demonstrates the use of the @ref FPSDK_COMMON_FPL
 
     See @ref FPSDK_BUILD_DOC on how to build and run this app, or @ref FPSDK_RUN_DOC on how to run it using pre-built
     Docker images.
 
+
     @section FPSDK_APPS_FPLTOOL_HELP Command line help
 
     @include fpltool_helpscreen.txt
 
-    */
+*/
 
 /* ****************************************************************************************************************** */
 // clang-format on